1//
2// Academic License - for use in teaching, academic research, and meeting
3// course requirements at degree granting institutions only. Not for
4// government, commercial, or other organizational use.
5//
6// File: cartesian_waypoints_planner.cpp
7//
8// Code generated for Simulink model 'cartesian_waypoints_planner'.
9//
10// Model version : 1.183
11// Simulink Coder version : 9.3 (R2020a) 18-Nov-2019
12// C/C++ source code generated on : Wed May 27 16:30:19 2020
13//
14// Target selection: ert.tlc
15// Embedded hardware selection: Generic->Unspecified (assume 32-bit Generic)
16// Code generation objectives: Unspecified
17// Validation result: Not run
18//
19#include "cartesian_waypoints_planner.h"
20#include "cartesian_waypoints_planner_private.h"
21
22// Block signals (default storage)
23B_cartesian_waypoints_planner_T cartesian_waypoints_planner_B;
24
25// Block states (default storage)
26DW_cartesian_waypoints_planne_T cartesian_waypoints_planner_DW;
27
28// Real-time model
29RT_MODEL_cartesian_waypoints__T cartesian_waypoints_planner_M_ =
30 RT_MODEL_cartesian_waypoints__T();
31RT_MODEL_cartesian_waypoints__T *const cartesian_waypoints_planner_M =
32 &cartesian_waypoints_planner_M_;
33
34// Forward declaration for local functions
35static void car_quaternioncg_parenReference(real_T obj_a, real_T obj_b, real_T
36 obj_c, real_T obj_d, real_T *o_a, real_T *o_b, real_T *o_c, real_T *o_d);
37static void cartesian_waypoi_emxInit_real_T(emxArray_real_T_cartesian_way_T
38 **pEmxArray, int32_T numDimensions);
39static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_way_T
40 *emxArray, int32_T oldNumel);
41static void cartesian_waypoi_emxInit_char_T(emxArray_char_T_cartesian_way_T
42 **pEmxArray, int32_T numDimensions);
43static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_way_T
44 *emxArray, int32_T oldNumel);
45static void cartesian_waypoi_emxFree_real_T(emxArray_real_T_cartesian_way_T
46 **pEmxArray);
47static void cartesian_waypoi_emxFree_char_T(emxArray_char_T_cartesian_way_T
48 **pEmxArray);
49static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
50 f_robotics_manip_internal_IKE_T *iobj_0);
51static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
52 const real_T tform[16], const real_T weights[6]);
53static void RigidBodyTree_validateConfigu_e(x_robotics_manip_internal_Rig_T *obj,
54 real_T Q[6]);
55static boolean_T cartesian_waypoints_plan_strcmp(const
56 emxArray_char_T_cartesian_way_T *a, const emxArray_char_T_cartesian_way_T *b);
57static void rigidBodyJoint_get_JointAxis_e(const c_rigidBodyJoint_cartesian__e_T
58 *obj, real_T ax[3]);
59static void cartesian_waypoints_planner_cat(real_T varargin_1, real_T varargin_2,
60 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
61 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9]);
62static void rigidBodyJoint_transformBodyT_e(const
63 c_rigidBodyJoint_cartesian__e_T *obj, const real_T q_data[], const int32_T
64 *q_size, real_T T[16]);
65static void rigidBodyJoint_transformBodyToP(const
66 c_rigidBodyJoint_cartesian__e_T *obj, real_T T[16]);
67static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
68 const real_T qv[6], const emxArray_char_T_cartesian_way_T *body1Name, real_T
69 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_way_T *Jac);
70static creal_T cartesian_waypoints_planne_sqrt(const creal_T x);
71static real_T cartesian_waypoints_plann_xnrm2(int32_T n, const real_T x[9],
72 int32_T ix0);
73static real_T cartesian_waypoints_plann_xdotc(int32_T n, const real_T x[9],
74 int32_T ix0, const real_T y[9], int32_T iy0);
75static void cartesian_waypoints_plann_xaxpy(int32_T n, real_T a, int32_T ix0,
76 const real_T y[9], int32_T iy0, real_T b_y[9]);
77static real_T cartesian_waypoints_pla_xnrm2_e(const real_T x[3], int32_T ix0);
78static void cartesian_waypoints_p_xaxpy_evq(int32_T n, real_T a, const real_T x
79 [9], int32_T ix0, real_T y[3], int32_T iy0);
80static void cartesian_waypoints_pl_xaxpy_ev(int32_T n, real_T a, const real_T x
81 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9]);
82static void cartesian_waypoints_plann_xswap(const real_T x[9], int32_T ix0,
83 int32_T iy0, real_T b_x[9]);
84static void cartesian_waypoints_plann_xrotg(real_T a, real_T b, real_T *b_a,
85 real_T *b_b, real_T *c, real_T *s);
86static void cartesian_waypoints_planne_xrot(const real_T x[9], int32_T ix0,
87 int32_T iy0, real_T c, real_T s, real_T b_x[9]);
88static void cartesian_waypoints_planner_svd(const real_T A[9], real_T U[9],
89 real_T s[3], real_T V[9]);
90static void cartesian_waypoints__rotm2axang(const real_T R[9], real_T axang[4]);
91static void cartesian_IKHelpers_computeCost(const real_T x[6],
92 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
93 emxArray_real_T_cartesian_way_T *Jac, f_robotics_manip_internal_IKE_T **b_args);
94static void cartesian_waypoints_planner_eye(real_T b_I[36]);
95static void cartesian_way_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
96 **pEmxArray, int32_T numDimensions);
97static void cartesian_waypo_emxInit_int32_T(emxArray_int32_T_cartesian_wa_T
98 **pEmxArray, int32_T numDimensions);
99static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
100 *emxArray, int32_T oldNumel);
101static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_wa_T
102 *emxArray, int32_T oldNumel);
103static real_T cartesian_waypoints_plan_norm_e(const real_T x[6]);
104static real_T SystemTimeProvider_getElapsedTi(const
105 f_robotics_core_internal_Syst_T *obj);
106static real_T cartesian_waypoints_pl_xnrm2_ev(int32_T n, const
107 emxArray_real_T_cartesian_way_T *x, int32_T ix0);
108static void cartesian_waypoints_plan_qrpf_e(const
109 emxArray_real_T_cartesian_way_T *A, int32_T m, int32_T n,
110 emxArray_real_T_cartesian_way_T *tau, const emxArray_int32_T_cartesian_wa_T
111 *jpvt, emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T
112 *b_jpvt);
113static void cartesian_waypoints_pla_xzgetrf(int32_T m, int32_T n, const
114 emxArray_real_T_cartesian_way_T *A, int32_T lda,
115 emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T *ipiv,
116 int32_T *info);
117static void cartesian_waypoints_plann_xtrsm(int32_T m, int32_T n, const
118 emxArray_real_T_cartesian_way_T *A, int32_T lda, const
119 emxArray_real_T_cartesian_way_T *B, int32_T ldb,
120 emxArray_real_T_cartesian_way_T *b_B);
121static void cartesian_waypo_emxFree_int32_T(emxArray_int32_T_cartesian_wa_T
122 **pEmxArray);
123static void cartesian_waypoints_pl_mldivide(const
124 emxArray_real_T_cartesian_way_T *A, const emxArray_real_T_cartesian_way_T *B,
125 emxArray_real_T_cartesian_way_T *Y);
126static void cartesian_way_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
127 **pEmxArray);
128static boolean_T DampedBFGSwGradientProjection_a(const
129 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
130 emxArray_real_T_cartesian_way_T *alpha);
131static void cartesian_waypoints_planner_inv(const
132 emxArray_real_T_cartesian_way_T *x, emxArray_real_T_cartesian_way_T *y);
133static void cartesian_waypoints_planne_diag(const
134 emxArray_real_T_cartesian_way_T *v, emxArray_real_T_cartesian_way_T *d);
135static void cartesian_waypoints_pla_sqrt_ev(emxArray_real_T_cartesian_way_T *x);
136static boolean_T cartesian_waypoints_planner_any(const
137 emxArray_boolean_T_cartesian__T *x);
138static boolean_T cartesian_wa_isPositiveDefinite(const real_T B[36]);
139static boolean_T DampedBFGSwGradientProjection_e(const
140 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6]);
141static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
142 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
143 *iter);
144static void cartesian_waypoints_pl_isfinite(const
145 emxArray_real_T_cartesian_way_T *x, emxArray_boolean_T_cartesian__T *b);
146static void cartesi_genrand_uint32_vector_e(uint32_T mt[625], uint32_T u[2]);
147static boolean_T cartesian_waypoi_is_valid_state(const uint32_T mt[625]);
148static real_T cartesian_waypoints__genrandu_e(uint32_T mt[625]);
149static real_T cartesia_eml_rand_mt19937ar_evq(uint32_T state[625]);
150static void cartesian_waypoints_plann_randn(const real_T varargin_1[2],
151 emxArray_real_T_cartesian_way_T *r);
152static void cartesian__eml_rand_mt19937ar_e(const uint32_T state[625], uint32_T
153 b_state[625], real_T *r);
154static void cartesian_waypoints_plan_rand_e(real_T varargin_1,
155 emxArray_real_T_cartesian_way_T *r);
156static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
157 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
158 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
159 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
160 solutionInfo_Status_size[2]);
161static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
162 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
163 real_T QSol[6]);
164static void cartesian_w_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
165 **pEmxArray, int32_T numDimensions);
166static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
167 *emxArray, int32_T oldNumel);
168static void ca_rigidBodyJoint_get_JointAxis(const
169 c_rigidBodyJoint_cartesian_wa_T *obj, real_T ax[3]);
170static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,
171 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree);
172static void cartesian_w_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
173 **pEmxArray);
174static void matlabCodegenHandle_matlabCodeg(ros_slros_internal_block_GetP_T *obj);
175static void matlabCodegenHandle_matlab_evqu(robotics_slmanip_internal_b_e_T *obj);
176static void cartesian_wa_SystemCore_release(b_inverseKinematics_cartesian_T *obj);
177static void cartesian_SystemCore_delete_evq(b_inverseKinematics_cartesian_T *obj);
178static void matlabCodegenHandle_matlabC_evq(b_inverseKinematics_cartesian_T *obj);
179static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian__e_T
180 *pStruct);
181static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
182 *pStruct);
183static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
184 *pStruct);
185static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
186 *pStruct);
187static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_b_e_T
188 *pStruct);
189static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
190 *pStruct);
191static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
192 *pStruct);
193static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
194 *pStruct);
195static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
196 *pStruct);
197static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_wa_T
198 *pStruct);
199static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
200 *pStruct);
201static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
202 *pStruct);
203static void emxFreeStruct_robotics_slmani_e(robotics_slmanip_internal_blo_T
204 *pStruct);
205static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
206 *pStruct);
207static void matlabCodegenHandle_matl_evqusn(ros_slros_internal_block_Subs_T *obj);
208static void cartesian_way_matlabCodegenHa_j(ros_slros_internal_block_SetP_T *obj);
209static void matlabCodegenHandle_matla_evqus(ros_slros_internal_block_Publ_T *obj);
210static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian__e_T
211 *pStruct);
212static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
213 *pStruct);
214static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
215 *pStruct);
216static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
217 *pStruct);
218static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_b_e_T
219 *pStruct);
220static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
221 *pStruct);
222static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
223 *pStruct);
224static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
225 *pStruct);
226static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
227 *pStruct);
228static void cartesia_twister_state_vector_e(uint32_T mt[625]);
229static void cartesian_wa_eml_rand_mt19937ar(uint32_T state[625]);
230static v_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_evqus
231 (v_robotics_manip_internal_Rig_T *obj);
232static v_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_evqusn
233 (v_robotics_manip_internal_Rig_T *obj);
234static v_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_evqusng
235 (v_robotics_manip_internal_Rig_T *obj);
236static v_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_evqusngv
237 (v_robotics_manip_internal_Rig_T *obj);
238static v_robotics_manip_internal_Rig_T *c_RigidBody_RigidBody_evqusngvo
239 (v_robotics_manip_internal_Rig_T *obj);
240static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0
241 (v_robotics_manip_internal_Rig_T *obj);
242static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0a
243 (v_robotics_manip_internal_Rig_T *obj);
244static y_robotics_manip_internal_Rig_T *c_RigidBodyTree_RigidBodyTree_e
245 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
246 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
247 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
248 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
249 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
250 v_robotics_manip_internal_Rig_T *iobj_7);
251static void cartesian_waypoints_planne_rand(real_T r[5]);
252static w_robotics_manip_internal_Rig_T *RigidBody_RigidBod_evqusngvo0ar
253 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0);
254static w_robotics_manip_internal_Rig_T *RigidBody_RigidBo_evqusngvo0ar1
255 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0);
256static w_robotics_manip_internal_Rig_T *RigidBody_RigidB_evqusngvo0ar1d
257 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0);
258static w_robotics_manip_internal_Rig_T *RigidBody_Rigid_evqusngvo0ar1db
259 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0);
260static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_c
261 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0);
262static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
263 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
264 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
265 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
266 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
267 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian__e_T
268 *iobj_7, c_rigidBodyJoint_cartesian__e_T *iobj_8,
269 c_rigidBodyJoint_cartesian__e_T *iobj_9, c_rigidBodyJoint_cartesian__e_T
270 *iobj_10, c_rigidBodyJoint_cartesian__e_T *iobj_11,
271 c_rigidBodyJoint_cartesian__e_T *iobj_12, c_rigidBodyJoint_cartesian__e_T
272 *iobj_13, c_rigidBodyJoint_cartesian__e_T *iobj_14,
273 w_robotics_manip_internal_Rig_T *iobj_15);
274static x_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_ev
275 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
276 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
277 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
278 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
279 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
280 c_rigidBodyJoint_cartesian__e_T *iobj_7, c_rigidBodyJoint_cartesian__e_T
281 *iobj_8, c_rigidBodyJoint_cartesian__e_T *iobj_9,
282 c_rigidBodyJoint_cartesian__e_T *iobj_10, c_rigidBodyJoint_cartesian__e_T
283 *iobj_11, c_rigidBodyJoint_cartesian__e_T *iobj_12,
284 c_rigidBodyJoint_cartesian__e_T *iobj_13, c_rigidBodyJoint_cartesian__e_T
285 *iobj_14, c_rigidBodyJoint_cartesian__e_T *iobj_15,
286 w_robotics_manip_internal_Rig_T *iobj_16);
287static c_rigidBodyJoint_cartesian__e_T *c_rigidBodyJoint_rigidBodyJoint
288 (c_rigidBodyJoint_cartesian__e_T *obj, const emxArray_char_T_cartesian_way_T
289 *jname, const emxArray_char_T_cartesian_way_T *jtype);
290static w_robotics_manip_internal_Rig_T *cartesian_waypoi_RigidBody_copy(const
291 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0,
292 c_rigidBodyJoint_cartesian__e_T *iobj_1, w_robotics_manip_internal_Rig_T
293 *iobj_2);
294static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
295 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_way_T
296 *parentName, c_rigidBodyJoint_cartesian__e_T *iobj_0,
297 c_rigidBodyJoint_cartesian__e_T *iobj_1, w_robotics_manip_internal_Rig_T
298 *iobj_2);
299static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
300 y_robotics_manip_internal_Rig_T *rigidbodytree,
301 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
302 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
303 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
304 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
305 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
306 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
307 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
308 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
309 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
310 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
311 c_rigidBodyJoint_cartesian__e_T *iobj_15, c_rigidBodyJoint_cartesian__e_T
312 *iobj_16, c_rigidBodyJoint_cartesian__e_T *iobj_17,
313 c_rigidBodyJoint_cartesian__e_T *iobj_18, c_rigidBodyJoint_cartesian__e_T
314 *iobj_19, c_rigidBodyJoint_cartesian__e_T *iobj_20,
315 c_rigidBodyJoint_cartesian__e_T *iobj_21, c_rigidBodyJoint_cartesian__e_T
316 *iobj_22, c_rigidBodyJoint_cartesian__e_T *iobj_23,
317 c_rigidBodyJoint_cartesian__e_T *iobj_24, c_rigidBodyJoint_cartesian__e_T
318 *iobj_25, c_rigidBodyJoint_cartesian__e_T *iobj_26,
319 c_rigidBodyJoint_cartesian__e_T *iobj_27, c_rigidBodyJoint_cartesian__e_T
320 *iobj_28, c_rigidBodyJoint_cartesian__e_T *iobj_29,
321 c_rigidBodyJoint_cartesian__e_T *iobj_30, c_rigidBodyJoint_cartesian__e_T
322 *iobj_31, c_rigidBodyJoint_cartesian__e_T *iobj_32,
323 c_rigidBodyJoint_cartesian__e_T *iobj_33, c_rigidBodyJoint_cartesian__e_T
324 *iobj_34, c_rigidBodyJoint_cartesian__e_T *iobj_35,
325 c_rigidBodyJoint_cartesian__e_T *iobj_36, c_rigidBodyJoint_cartesian__e_T
326 *iobj_37, c_rigidBodyJoint_cartesian__e_T *iobj_38,
327 c_rigidBodyJoint_cartesian__e_T *iobj_39, w_robotics_manip_internal_Rig_T
328 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41);
329static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
330 (h_robotics_core_internal_Damp_T *obj);
331static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_wa_T
332 *pStruct);
333static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
334 *pStruct);
335static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
336 *pStruct);
337static void emxInitStruct_robotics_slmani_e(robotics_slmanip_internal_blo_T
338 *pStruct);
339static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
340 *pStruct);
341static n_robotics_manip_internal_Rig_T *cartesian_w_RigidBody_RigidBody
342 (n_robotics_manip_internal_Rig_T *obj);
343static n_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_e
344 (n_robotics_manip_internal_Rig_T *obj);
345static n_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_ev
346 (n_robotics_manip_internal_Rig_T *obj);
347static n_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_evq
348 (n_robotics_manip_internal_Rig_T *obj);
349static n_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_evqu
350 (n_robotics_manip_internal_Rig_T *obj);
351static p_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
352 (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,
353 n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T
354 *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,
355 n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T
356 *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,
357 n_robotics_manip_internal_Rig_T *iobj_7);
358int32_T div_s32_floor(int32_T numerator, int32_T denominator)
359{
360 int32_T quotient;
361 uint32_T absNumerator;
362 uint32_T absDenominator;
363 uint32_T tempAbsQuotient;
364 boolean_T quotientNeedsNegation;
365 if (denominator == 0) {
366 quotient = numerator >= 0 ? MAX_int32_T : MIN_int32_T;
367
368 // Divide by zero handler
369 } else {
370 absNumerator = numerator < 0 ? ~static_cast<uint32_T>(numerator) + 1U :
371 static_cast<uint32_T>(numerator);
372 absDenominator = denominator < 0 ? ~static_cast<uint32_T>(denominator) + 1U :
373 static_cast<uint32_T>(denominator);
374 quotientNeedsNegation = ((numerator < 0) != (denominator < 0));
375 tempAbsQuotient = absNumerator / absDenominator;
376 if (quotientNeedsNegation) {
377 absNumerator %= absDenominator;
378 if (absNumerator > 0U) {
379 tempAbsQuotient++;
380 }
381 }
382
383 quotient = quotientNeedsNegation ? -static_cast<int32_T>(tempAbsQuotient) :
384 static_cast<int32_T>(tempAbsQuotient);
385 }
386
387 return quotient;
388}
389
390// Function for MATLAB Function: '<Root>/MATLAB Function'
391static void car_quaternioncg_parenReference(real_T obj_a, real_T obj_b, real_T
392 obj_c, real_T obj_d, real_T *o_a, real_T *o_b, real_T *o_c, real_T *o_d)
393{
394 *o_a = obj_a;
395 *o_b = obj_b;
396 *o_c = obj_c;
397 *o_d = obj_d;
398}
399
400static void cartesian_waypoi_emxInit_real_T(emxArray_real_T_cartesian_way_T
401 **pEmxArray, int32_T numDimensions)
402{
403 emxArray_real_T_cartesian_way_T *emxArray;
404 *pEmxArray = (emxArray_real_T_cartesian_way_T *)malloc(sizeof
405 (emxArray_real_T_cartesian_way_T));
406 emxArray = *pEmxArray;
407 emxArray->data = (real_T *)NULL;
408 emxArray->numDimensions = numDimensions;
409 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
410 emxArray->allocatedSize = 0;
411 emxArray->canFreeData = true;
412 for (cartesian_waypoints_planner_B.i_i = 0; cartesian_waypoints_planner_B.i_i <
413 numDimensions; cartesian_waypoints_planner_B.i_i++) {
414 emxArray->size[cartesian_waypoints_planner_B.i_i] = 0;
415 }
416}
417
418static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_way_T
419 *emxArray, int32_T oldNumel)
420{
421 void *newData;
422 if (oldNumel < 0) {
423 oldNumel = 0;
424 }
425
426 cartesian_waypoints_planner_B.newNumel_m = 1;
427 for (cartesian_waypoints_planner_B.i_ft = 0;
428 cartesian_waypoints_planner_B.i_ft < emxArray->numDimensions;
429 cartesian_waypoints_planner_B.i_ft++) {
430 cartesian_waypoints_planner_B.newNumel_m *= emxArray->
431 size[cartesian_waypoints_planner_B.i_ft];
432 }
433
434 if (cartesian_waypoints_planner_B.newNumel_m > emxArray->allocatedSize) {
435 cartesian_waypoints_planner_B.i_ft = emxArray->allocatedSize;
436 if (cartesian_waypoints_planner_B.i_ft < 16) {
437 cartesian_waypoints_planner_B.i_ft = 16;
438 }
439
440 while (cartesian_waypoints_planner_B.i_ft <
441 cartesian_waypoints_planner_B.newNumel_m) {
442 if (cartesian_waypoints_planner_B.i_ft > 1073741823) {
443 cartesian_waypoints_planner_B.i_ft = MAX_int32_T;
444 } else {
445 cartesian_waypoints_planner_B.i_ft <<= 1;
446 }
447 }
448
449 newData = calloc(static_cast<uint32_T>(cartesian_waypoints_planner_B.i_ft),
450 sizeof(real_T));
451 if (emxArray->data != NULL) {
452 memcpy(newData, emxArray->data, sizeof(real_T) * oldNumel);
453 if (emxArray->canFreeData) {
454 free(emxArray->data);
455 }
456 }
457
458 emxArray->data = (real_T *)newData;
459 emxArray->allocatedSize = cartesian_waypoints_planner_B.i_ft;
460 emxArray->canFreeData = true;
461 }
462}
463
464static void cartesian_waypoi_emxInit_char_T(emxArray_char_T_cartesian_way_T
465 **pEmxArray, int32_T numDimensions)
466{
467 emxArray_char_T_cartesian_way_T *emxArray;
468 *pEmxArray = (emxArray_char_T_cartesian_way_T *)malloc(sizeof
469 (emxArray_char_T_cartesian_way_T));
470 emxArray = *pEmxArray;
471 emxArray->data = (char_T *)NULL;
472 emxArray->numDimensions = numDimensions;
473 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
474 emxArray->allocatedSize = 0;
475 emxArray->canFreeData = true;
476 for (cartesian_waypoints_planner_B.i_l = 0; cartesian_waypoints_planner_B.i_l <
477 numDimensions; cartesian_waypoints_planner_B.i_l++) {
478 emxArray->size[cartesian_waypoints_planner_B.i_l] = 0;
479 }
480}
481
482static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_way_T
483 *emxArray, int32_T oldNumel)
484{
485 void *newData;
486 if (oldNumel < 0) {
487 oldNumel = 0;
488 }
489
490 cartesian_waypoints_planner_B.newNumel = 1;
491 for (cartesian_waypoints_planner_B.i_h = 0; cartesian_waypoints_planner_B.i_h <
492 emxArray->numDimensions; cartesian_waypoints_planner_B.i_h++) {
493 cartesian_waypoints_planner_B.newNumel *= emxArray->
494 size[cartesian_waypoints_planner_B.i_h];
495 }
496
497 if (cartesian_waypoints_planner_B.newNumel > emxArray->allocatedSize) {
498 cartesian_waypoints_planner_B.i_h = emxArray->allocatedSize;
499 if (cartesian_waypoints_planner_B.i_h < 16) {
500 cartesian_waypoints_planner_B.i_h = 16;
501 }
502
503 while (cartesian_waypoints_planner_B.i_h <
504 cartesian_waypoints_planner_B.newNumel) {
505 if (cartesian_waypoints_planner_B.i_h > 1073741823) {
506 cartesian_waypoints_planner_B.i_h = MAX_int32_T;
507 } else {
508 cartesian_waypoints_planner_B.i_h <<= 1;
509 }
510 }
511
512 newData = calloc(static_cast<uint32_T>(cartesian_waypoints_planner_B.i_h),
513 sizeof(char_T));
514 if (emxArray->data != NULL) {
515 memcpy(newData, emxArray->data, sizeof(char_T) * oldNumel);
516 if (emxArray->canFreeData) {
517 free(emxArray->data);
518 }
519 }
520
521 emxArray->data = (char_T *)newData;
522 emxArray->allocatedSize = cartesian_waypoints_planner_B.i_h;
523 emxArray->canFreeData = true;
524 }
525}
526
527static void cartesian_waypoi_emxFree_real_T(emxArray_real_T_cartesian_way_T
528 **pEmxArray)
529{
530 if (*pEmxArray != (emxArray_real_T_cartesian_way_T *)NULL) {
531 if (((*pEmxArray)->data != (real_T *)NULL) && (*pEmxArray)->canFreeData) {
532 free((*pEmxArray)->data);
533 }
534
535 free((*pEmxArray)->size);
536 free(*pEmxArray);
537 *pEmxArray = (emxArray_real_T_cartesian_way_T *)NULL;
538 }
539}
540
541static void cartesian_waypoi_emxFree_char_T(emxArray_char_T_cartesian_way_T
542 **pEmxArray)
543{
544 if (*pEmxArray != (emxArray_char_T_cartesian_way_T *)NULL) {
545 if (((*pEmxArray)->data != (char_T *)NULL) && (*pEmxArray)->canFreeData) {
546 free((*pEmxArray)->data);
547 }
548
549 free((*pEmxArray)->size);
550 free(*pEmxArray);
551 *pEmxArray = (emxArray_char_T_cartesian_way_T *)NULL;
552 }
553}
554
555static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
556 f_robotics_manip_internal_IKE_T *iobj_0)
557{
558 real_T n;
559 emxArray_real_T_cartesian_way_T *A;
560 emxArray_real_T_cartesian_way_T *b;
561 real_T m;
562 c_rigidBodyJoint_cartesian__e_T *joint;
563 real_T pnum;
564 int32_T d;
565 int32_T b_i;
566 emxArray_real_T_cartesian_way_T *e;
567 int32_T j;
568 int32_T p;
569 emxArray_real_T_cartesian_way_T *s;
570 x_robotics_manip_internal_Rig_T *obj_0;
571 w_robotics_manip_internal_Rig_T *body;
572 int32_T c;
573 boolean_T b_bool;
574 emxArray_char_T_cartesian_way_T *a;
575 int32_T b_kstr;
576 char_T b_0[5];
577 int8_T b_I[16];
578 int32_T m_0;
579 real_T t;
580 real_T pnum_0;
581 int32_T loop_ub;
582 real_T tmp;
583 real_T w;
584 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
585
586 int32_T exitg1;
587 cartesian_waypoi_emxInit_real_T(&A, 2);
588 n = obj->RigidBodyTreeInternal->PositionNumber;
589 b_kstr = A->size[0] * A->size[1];
590 c = static_cast<int32_T>(n);
591 A->size[0] = c;
592 loop_ub = static_cast<int32_T>(2.0 * n);
593 A->size[1] = loop_ub;
594 cartes_emxEnsureCapacity_real_T(A, b_kstr);
595 m_0 = loop_ub * c - 1;
596 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
597 A->data[b_kstr] = 0.0;
598 }
599
600 cartesian_waypoi_emxInit_real_T(&b, 1);
601 b_kstr = b->size[0];
602 b->size[0] = loop_ub;
603 cartes_emxEnsureCapacity_real_T(b, b_kstr);
604 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
605 b->data[b_kstr] = 0.0;
606 }
607
608 n = 1.0;
609 m = 1.0;
610 pnum = obj->RigidBodyTreeInternal->NumBodies;
611 d = static_cast<int32_T>(pnum) - 1;
612 cartesian_waypoi_emxInit_real_T(&e, 2);
613 cartesian_waypoi_emxInit_real_T(&s, 2);
614 cartesian_waypoi_emxInit_char_T(&a, 2);
615 if (0 <= d) {
616 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
617 b_0[b_kstr] = tmp_0[b_kstr];
618 }
619 }
620
621 for (b_i = 0; b_i <= d; b_i++) {
622 body = obj->RigidBodyTreeInternal->Bodies[b_i];
623 joint = body->JointInternal;
624 pnum = joint->PositionNumber;
625 b_kstr = a->size[0] * a->size[1];
626 a->size[0] = 1;
627 a->size[1] = joint->Type->size[1];
628 cartes_emxEnsureCapacity_char_T(a, b_kstr);
629 loop_ub = joint->Type->size[0] * joint->Type->size[1] - 1;
630 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
631 a->data[b_kstr] = joint->Type->data[b_kstr];
632 }
633
634 b_bool = false;
635 if (a->size[1] == 5) {
636 b_kstr = 1;
637 do {
638 exitg1 = 0;
639 if (b_kstr - 1 < 5) {
640 loop_ub = b_kstr - 1;
641 if (a->data[loop_ub] != b_0[loop_ub]) {
642 exitg1 = 1;
643 } else {
644 b_kstr++;
645 }
646 } else {
647 b_bool = true;
648 exitg1 = 1;
649 }
650 } while (exitg1 == 0);
651 }
652
653 if (!b_bool) {
654 tmp = (n + pnum) - 1.0;
655 if (n > tmp) {
656 j = 0;
657 } else {
658 j = static_cast<int32_T>(n) - 1;
659 }
660
661 w = m + pnum;
662 if (m > w - 1.0) {
663 p = 0;
664 } else {
665 p = static_cast<int32_T>(m) - 1;
666 }
667
668 if (pnum < 0.0) {
669 t = 0.0;
670 pnum_0 = 0.0;
671 } else {
672 t = pnum;
673 pnum_0 = pnum;
674 }
675
676 m_0 = static_cast<int32_T>(pnum_0) - 1;
677 b_kstr = s->size[0] * s->size[1];
678 c = static_cast<int32_T>(t);
679 s->size[0] = c;
680 s->size[1] = c;
681 cartes_emxEnsureCapacity_real_T(s, b_kstr);
682 loop_ub = c * c - 1;
683 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
684 s->data[b_kstr] = 0.0;
685 }
686
687 if (c > 0) {
688 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
689 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
690 }
691 }
692
693 loop_ub = s->size[1];
694 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
695 m_0 = s->size[0];
696 for (c = 0; c < m_0; c++) {
697 A->data[(j + c) + A->size[0] * (p + b_kstr)] = s->data[s->size[0] *
698 b_kstr + c];
699 }
700 }
701
702 if (n > tmp) {
703 j = 0;
704 } else {
705 j = static_cast<int32_T>(n) - 1;
706 }
707
708 tmp = 2.0 * pnum + m;
709 if (w > tmp - 1.0) {
710 p = 0;
711 } else {
712 p = static_cast<int32_T>(w) - 1;
713 }
714
715 if (pnum < 0.0) {
716 t = 0.0;
717 pnum_0 = 0.0;
718 } else {
719 t = pnum;
720 pnum_0 = pnum;
721 }
722
723 m_0 = static_cast<int32_T>(pnum_0) - 1;
724 b_kstr = s->size[0] * s->size[1];
725 c = static_cast<int32_T>(t);
726 s->size[0] = c;
727 s->size[1] = c;
728 cartes_emxEnsureCapacity_real_T(s, b_kstr);
729 loop_ub = c * c - 1;
730 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
731 s->data[b_kstr] = 0.0;
732 }
733
734 if (c > 0) {
735 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
736 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
737 }
738 }
739
740 loop_ub = s->size[1];
741 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
742 m_0 = s->size[0];
743 for (c = 0; c < m_0; c++) {
744 A->data[(j + c) + A->size[0] * (p + b_kstr)] = -s->data[s->size[0] *
745 b_kstr + c];
746 }
747 }
748
749 b_kstr = e->size[0] * e->size[1];
750 e->size[0] = joint->PositionLimitsInternal->size[0];
751 e->size[1] = 2;
752 cartes_emxEnsureCapacity_real_T(e, b_kstr);
753 loop_ub = joint->PositionLimitsInternal->size[0] *
754 joint->PositionLimitsInternal->size[1] - 1;
755 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
756 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
757 }
758
759 b->data[static_cast<int32_T>(m) - 1] = e->data[1];
760 b_kstr = e->size[0] * e->size[1];
761 e->size[0] = joint->PositionLimitsInternal->size[0];
762 e->size[1] = 2;
763 cartes_emxEnsureCapacity_real_T(e, b_kstr);
764 loop_ub = joint->PositionLimitsInternal->size[0] *
765 joint->PositionLimitsInternal->size[1] - 1;
766 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
767 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
768 }
769
770 b->data[static_cast<int32_T>(m + 1.0) - 1] = -e->data[0];
771 m = tmp;
772 }
773
774 n += pnum;
775 }
776
777 cartesian_waypoi_emxFree_real_T(&s);
778 b_kstr = A->size[0] * A->size[1];
779 c = obj->Solver->ConstraintMatrix->size[0] * obj->Solver->
780 ConstraintMatrix->size[1];
781 obj->Solver->ConstraintMatrix->size[0] = A->size[0];
782 obj->Solver->ConstraintMatrix->size[1] = A->size[1];
783 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintMatrix, c);
784 loop_ub = b_kstr - 1;
785 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
786 obj->Solver->ConstraintMatrix->data[b_kstr] = A->data[b_kstr];
787 }
788
789 cartesian_waypoi_emxFree_real_T(&A);
790 b_kstr = obj->Solver->ConstraintBound->size[0];
791 obj->Solver->ConstraintBound->size[0] = b->size[0];
792 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintBound, b_kstr);
793 loop_ub = b->size[0];
794 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
795 obj->Solver->ConstraintBound->data[b_kstr] = b->data[b_kstr];
796 }
797
798 obj_0 = obj->RigidBodyTreeInternal;
799 b_kstr = e->size[0] * e->size[1];
800 e->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
801 e->size[1] = 2;
802 cartes_emxEnsureCapacity_real_T(e, b_kstr);
803 loop_ub = (static_cast<int32_T>(obj_0->PositionNumber) << 1) - 1;
804 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
805 e->data[b_kstr] = 0.0;
806 }
807
808 n = 1.0;
809 m = obj_0->NumBodies;
810 c = static_cast<int32_T>(m) - 1;
811 if (0 <= c) {
812 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
813 b_0[b_kstr] = tmp_0[b_kstr];
814 }
815 }
816
817 for (b_i = 0; b_i <= c; b_i++) {
818 body = obj_0->Bodies[b_i];
819 b_kstr = a->size[0] * a->size[1];
820 a->size[0] = 1;
821 a->size[1] = body->JointInternal->Type->size[1];
822 cartes_emxEnsureCapacity_char_T(a, b_kstr);
823 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
824 Type->size[1] - 1;
825 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
826 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
827 }
828
829 b_bool = false;
830 if (a->size[1] == 5) {
831 b_kstr = 1;
832 do {
833 exitg1 = 0;
834 if (b_kstr - 1 < 5) {
835 loop_ub = b_kstr - 1;
836 if (a->data[loop_ub] != b_0[loop_ub]) {
837 exitg1 = 1;
838 } else {
839 b_kstr++;
840 }
841 } else {
842 b_bool = true;
843 exitg1 = 1;
844 }
845 } while (exitg1 == 0);
846 }
847
848 if (!b_bool) {
849 pnum = body->JointInternal->PositionNumber;
850 tmp = n + pnum;
851 if (n > tmp - 1.0) {
852 d = 0;
853 } else {
854 d = static_cast<int32_T>(n) - 1;
855 }
856
857 joint = body->JointInternal;
858 loop_ub = joint->PositionLimitsInternal->size[0];
859 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
860 e->data[d + b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
861 }
862
863 loop_ub = joint->PositionLimitsInternal->size[0];
864 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
865 e->data[(d + b_kstr) + e->size[0]] = joint->PositionLimitsInternal->
866 data[b_kstr + joint->PositionLimitsInternal->size[0]];
867 }
868
869 n = tmp;
870 }
871 }
872
873 cartesian_waypoi_emxFree_char_T(&a);
874 b_kstr = obj->Limits->size[0] * obj->Limits->size[1];
875 obj->Limits->size[0] = e->size[0];
876 obj->Limits->size[1] = 2;
877 cartes_emxEnsureCapacity_real_T(obj->Limits, b_kstr);
878 loop_ub = e->size[0] * e->size[1] - 1;
879 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
880 obj->Limits->data[b_kstr] = e->data[b_kstr];
881 }
882
883 cartesian_waypoi_emxFree_real_T(&e);
884 obj->Solver->ExtraArgs = iobj_0;
885 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
886 obj->Solver->ExtraArgs->WeightMatrix[b_kstr] = 0.0;
887 }
888
889 obj->Solver->ExtraArgs->Robot = obj->RigidBodyTreeInternal;
890 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
891 b_I[b_kstr] = 0;
892 }
893
894 b_I[0] = 1;
895 b_I[5] = 1;
896 b_I[10] = 1;
897 b_I[15] = 1;
898 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
899 obj->Solver->ExtraArgs->Tform[b_kstr] = b_I[b_kstr];
900 }
901
902 obj->Solver->ExtraArgs->BodyName->size[0] = 1;
903 obj->Solver->ExtraArgs->BodyName->size[1] = 0;
904 b_kstr = obj->Solver->ExtraArgs->ErrTemp->size[0];
905 obj->Solver->ExtraArgs->ErrTemp->size[0] = 6;
906 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->ErrTemp, b_kstr);
907 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
908 obj->Solver->ExtraArgs->ErrTemp->data[b_kstr] = 0.0;
909 }
910
911 obj->Solver->ExtraArgs->CostTemp = 0.0;
912 b_kstr = b->size[0];
913 b->size[0] = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
914 cartes_emxEnsureCapacity_real_T(b, b_kstr);
915 loop_ub = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
916 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
917 b->data[b_kstr] = 0.0;
918 }
919
920 b_kstr = obj->Solver->ExtraArgs->GradTemp->size[0];
921 obj->Solver->ExtraArgs->GradTemp->size[0] = b->size[0];
922 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->GradTemp, b_kstr);
923 loop_ub = b->size[0];
924 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
925 obj->Solver->ExtraArgs->GradTemp->data[b_kstr] = b->data[b_kstr];
926 }
927
928 cartesian_waypoi_emxFree_real_T(&b);
929}
930
931static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
932 const real_T tform[16], const real_T weights[6])
933{
934 real_T weightMatrix[36];
935 f_robotics_manip_internal_IKE_T *args;
936 int32_T b_j;
937 static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
938 'e', 'e' };
939
940 memset(&weightMatrix[0], 0, 36U * sizeof(real_T));
941 for (b_j = 0; b_j < 6; b_j++) {
942 weightMatrix[b_j + 6 * b_j] = weights[b_j];
943 }
944
945 args = obj->Solver->ExtraArgs;
946 for (b_j = 0; b_j < 36; b_j++) {
947 args->WeightMatrix[b_j] = weightMatrix[b_j];
948 }
949
950 b_j = args->BodyName->size[0] * args->BodyName->size[1];
951 args->BodyName->size[0] = 1;
952 args->BodyName->size[1] = 11;
953 cartes_emxEnsureCapacity_char_T(args->BodyName, b_j);
954 for (b_j = 0; b_j < 11; b_j++) {
955 args->BodyName->data[b_j] = tmp[b_j];
956 }
957
958 for (b_j = 0; b_j < 16; b_j++) {
959 args->Tform[b_j] = tform[b_j];
960 }
961}
962
963static void RigidBodyTree_validateConfigu_e(x_robotics_manip_internal_Rig_T *obj,
964 real_T Q[6])
965{
966 emxArray_real_T_cartesian_way_T *limits;
967 boolean_T ubOK[6];
968 boolean_T lbOK[6];
969 real_T k;
970 w_robotics_manip_internal_Rig_T *body;
971 real_T pnum;
972 int32_T c;
973 int32_T f;
974 int32_T ii_data[6];
975 boolean_T b_bool;
976 emxArray_char_T_cartesian_way_T *a;
977 int32_T b_kstr;
978 char_T b[5];
979 c_rigidBodyJoint_cartesian__e_T *obj_0;
980 int32_T idx;
981 int32_T loop_ub;
982 emxArray_real_T_cartesian_way_T *limits_0;
983 emxArray_real_T_cartesian_way_T *limits_1;
984 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
985
986 boolean_T guard1 = false;
987 int32_T exitg1;
988 boolean_T exitg2;
989 cartesian_waypoi_emxInit_real_T(&limits, 2);
990 b_kstr = limits->size[0] * limits->size[1];
991 limits->size[0] = static_cast<int32_T>(obj->PositionNumber);
992 limits->size[1] = 2;
993 cartes_emxEnsureCapacity_real_T(limits, b_kstr);
994 loop_ub = (static_cast<int32_T>(obj->PositionNumber) << 1) - 1;
995 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
996 limits->data[b_kstr] = 0.0;
997 }
998
999 k = 1.0;
1000 pnum = obj->NumBodies;
1001 c = static_cast<int32_T>(pnum) - 1;
1002 cartesian_waypoi_emxInit_char_T(&a, 2);
1003 if (0 <= c) {
1004 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
1005 b[b_kstr] = tmp[b_kstr];
1006 }
1007 }
1008
1009 for (idx = 0; idx <= c; idx++) {
1010 body = obj->Bodies[idx];
1011 b_kstr = a->size[0] * a->size[1];
1012 a->size[0] = 1;
1013 a->size[1] = body->JointInternal->Type->size[1];
1014 cartes_emxEnsureCapacity_char_T(a, b_kstr);
1015 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
1016 Type->size[1] - 1;
1017 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1018 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
1019 }
1020
1021 b_bool = false;
1022 if (a->size[1] == 5) {
1023 b_kstr = 1;
1024 do {
1025 exitg1 = 0;
1026 if (b_kstr - 1 < 5) {
1027 loop_ub = b_kstr - 1;
1028 if (a->data[loop_ub] != b[loop_ub]) {
1029 exitg1 = 1;
1030 } else {
1031 b_kstr++;
1032 }
1033 } else {
1034 b_bool = true;
1035 exitg1 = 1;
1036 }
1037 } while (exitg1 == 0);
1038 }
1039
1040 if (!b_bool) {
1041 pnum = body->JointInternal->PositionNumber;
1042 pnum += k;
1043 if (k > pnum - 1.0) {
1044 f = 0;
1045 } else {
1046 f = static_cast<int32_T>(k) - 1;
1047 }
1048
1049 obj_0 = body->JointInternal;
1050 loop_ub = obj_0->PositionLimitsInternal->size[0];
1051 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1052 limits->data[f + b_kstr] = obj_0->PositionLimitsInternal->data[b_kstr];
1053 }
1054
1055 loop_ub = obj_0->PositionLimitsInternal->size[0];
1056 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1057 limits->data[(f + b_kstr) + limits->size[0]] =
1058 obj_0->PositionLimitsInternal->data[b_kstr +
1059 obj_0->PositionLimitsInternal->size[0]];
1060 }
1061
1062 k = pnum;
1063 }
1064 }
1065
1066 cartesian_waypoi_emxFree_char_T(&a);
1067 cartesian_waypoi_emxInit_real_T(&limits_0, 1);
1068 loop_ub = limits->size[0];
1069 b_kstr = limits_0->size[0];
1070 limits_0->size[0] = loop_ub;
1071 cartes_emxEnsureCapacity_real_T(limits_0, b_kstr);
1072 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1073 limits_0->data[b_kstr] = limits->data[b_kstr + limits->size[0]] +
1074 4.4408920985006262E-16;
1075 }
1076
1077 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1078 ubOK[b_kstr] = (Q[b_kstr] <= limits_0->data[b_kstr]);
1079 }
1080
1081 cartesian_waypoi_emxFree_real_T(&limits_0);
1082 cartesian_waypoi_emxInit_real_T(&limits_1, 1);
1083 loop_ub = limits->size[0];
1084 b_kstr = limits_1->size[0];
1085 limits_1->size[0] = loop_ub;
1086 cartes_emxEnsureCapacity_real_T(limits_1, b_kstr);
1087 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1088 limits_1->data[b_kstr] = limits->data[b_kstr] - 4.4408920985006262E-16;
1089 }
1090
1091 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1092 lbOK[b_kstr] = (Q[b_kstr] >= limits_1->data[b_kstr]);
1093 }
1094
1095 cartesian_waypoi_emxFree_real_T(&limits_1);
1096 b_bool = true;
1097 idx = 0;
1098 exitg2 = false;
1099 while ((!exitg2) && (idx < 6)) {
1100 if (!ubOK[idx]) {
1101 b_bool = false;
1102 exitg2 = true;
1103 } else {
1104 idx++;
1105 }
1106 }
1107
1108 guard1 = false;
1109 if (b_bool) {
1110 b_bool = true;
1111 idx = 0;
1112 exitg2 = false;
1113 while ((!exitg2) && (idx < 6)) {
1114 if (!lbOK[idx]) {
1115 b_bool = false;
1116 exitg2 = true;
1117 } else {
1118 idx++;
1119 }
1120 }
1121
1122 if (b_bool) {
1123 } else {
1124 guard1 = true;
1125 }
1126 } else {
1127 guard1 = true;
1128 }
1129
1130 if (guard1) {
1131 idx = 0;
1132 b_kstr = 1;
1133 exitg2 = false;
1134 while ((!exitg2) && (b_kstr - 1 < 6)) {
1135 if (!ubOK[b_kstr - 1]) {
1136 idx++;
1137 ii_data[idx - 1] = b_kstr;
1138 if (idx >= 6) {
1139 exitg2 = true;
1140 } else {
1141 b_kstr++;
1142 }
1143 } else {
1144 b_kstr++;
1145 }
1146 }
1147
1148 if (1 > idx) {
1149 idx = 0;
1150 }
1151
1152 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
1153 Q[ii_data[b_kstr] - 1] = limits->data[(ii_data[b_kstr] + limits->size[0])
1154 - 1];
1155 }
1156
1157 idx = 0;
1158 b_kstr = 1;
1159 exitg2 = false;
1160 while ((!exitg2) && (b_kstr - 1 < 6)) {
1161 if (!lbOK[b_kstr - 1]) {
1162 idx++;
1163 ii_data[idx - 1] = b_kstr;
1164 if (idx >= 6) {
1165 exitg2 = true;
1166 } else {
1167 b_kstr++;
1168 }
1169 } else {
1170 b_kstr++;
1171 }
1172 }
1173
1174 if (1 > idx) {
1175 idx = 0;
1176 }
1177
1178 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
1179 Q[ii_data[b_kstr] - 1] = limits->data[ii_data[b_kstr] - 1];
1180 }
1181 }
1182
1183 cartesian_waypoi_emxFree_real_T(&limits);
1184}
1185
1186static boolean_T cartesian_waypoints_plan_strcmp(const
1187 emxArray_char_T_cartesian_way_T *a, const emxArray_char_T_cartesian_way_T *b)
1188{
1189 boolean_T b_bool;
1190 int32_T kstr;
1191 int32_T b_kstr;
1192 boolean_T d;
1193 int32_T exitg1;
1194 b_bool = false;
1195 d = (a->size[1] == 0);
1196 if (d && (b->size[1] == 0)) {
1197 b_bool = true;
1198 } else if (a->size[1] != b->size[1]) {
1199 } else {
1200 b_kstr = 1;
1201 do {
1202 exitg1 = 0;
1203 if (b_kstr - 1 <= b->size[1] - 1) {
1204 kstr = b_kstr - 1;
1205 if (a->data[kstr] != b->data[kstr]) {
1206 exitg1 = 1;
1207 } else {
1208 b_kstr++;
1209 }
1210 } else {
1211 b_bool = true;
1212 exitg1 = 1;
1213 }
1214 } while (exitg1 == 0);
1215 }
1216
1217 return b_bool;
1218}
1219
1220static void rigidBodyJoint_get_JointAxis_e(const c_rigidBodyJoint_cartesian__e_T
1221 *obj, real_T ax[3])
1222{
1223 emxArray_char_T_cartesian_way_T *a;
1224 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1225
1226 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1227
1228 boolean_T guard1 = false;
1229 int32_T exitg1;
1230 cartesian_waypoi_emxInit_char_T(&a, 2);
1231 cartesian_waypoints_planner_B.b_kstr_n = a->size[0] * a->size[1];
1232 a->size[0] = 1;
1233 a->size[1] = obj->Type->size[1];
1234 cartes_emxEnsureCapacity_char_T(a, cartesian_waypoints_planner_B.b_kstr_n);
1235 cartesian_waypoints_planner_B.loop_ub_h = obj->Type->size[0] * obj->Type->
1236 size[1] - 1;
1237 for (cartesian_waypoints_planner_B.b_kstr_n = 0;
1238 cartesian_waypoints_planner_B.b_kstr_n <=
1239 cartesian_waypoints_planner_B.loop_ub_h;
1240 cartesian_waypoints_planner_B.b_kstr_n++) {
1241 a->data[cartesian_waypoints_planner_B.b_kstr_n] = obj->Type->
1242 data[cartesian_waypoints_planner_B.b_kstr_n];
1243 }
1244
1245 for (cartesian_waypoints_planner_B.b_kstr_n = 0;
1246 cartesian_waypoints_planner_B.b_kstr_n < 8;
1247 cartesian_waypoints_planner_B.b_kstr_n++) {
1248 cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.b_kstr_n] =
1249 tmp[cartesian_waypoints_planner_B.b_kstr_n];
1250 }
1251
1252 cartesian_waypoints_planner_B.b_bool_n = false;
1253 if (a->size[1] == 8) {
1254 cartesian_waypoints_planner_B.b_kstr_n = 1;
1255 do {
1256 exitg1 = 0;
1257 if (cartesian_waypoints_planner_B.b_kstr_n - 1 < 8) {
1258 cartesian_waypoints_planner_B.loop_ub_h =
1259 cartesian_waypoints_planner_B.b_kstr_n - 1;
1260 if (a->data[cartesian_waypoints_planner_B.loop_ub_h] !=
1261 cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.loop_ub_h])
1262 {
1263 exitg1 = 1;
1264 } else {
1265 cartesian_waypoints_planner_B.b_kstr_n++;
1266 }
1267 } else {
1268 cartesian_waypoints_planner_B.b_bool_n = true;
1269 exitg1 = 1;
1270 }
1271 } while (exitg1 == 0);
1272 }
1273
1274 guard1 = false;
1275 if (cartesian_waypoints_planner_B.b_bool_n) {
1276 guard1 = true;
1277 } else {
1278 cartesian_waypoints_planner_B.b_kstr_n = a->size[0] * a->size[1];
1279 a->size[0] = 1;
1280 a->size[1] = obj->Type->size[1];
1281 cartes_emxEnsureCapacity_char_T(a, cartesian_waypoints_planner_B.b_kstr_n);
1282 cartesian_waypoints_planner_B.loop_ub_h = obj->Type->size[0] * obj->
1283 Type->size[1] - 1;
1284 for (cartesian_waypoints_planner_B.b_kstr_n = 0;
1285 cartesian_waypoints_planner_B.b_kstr_n <=
1286 cartesian_waypoints_planner_B.loop_ub_h;
1287 cartesian_waypoints_planner_B.b_kstr_n++) {
1288 a->data[cartesian_waypoints_planner_B.b_kstr_n] = obj->Type->
1289 data[cartesian_waypoints_planner_B.b_kstr_n];
1290 }
1291
1292 for (cartesian_waypoints_planner_B.b_kstr_n = 0;
1293 cartesian_waypoints_planner_B.b_kstr_n < 9;
1294 cartesian_waypoints_planner_B.b_kstr_n++) {
1295 cartesian_waypoints_planner_B.b_p[cartesian_waypoints_planner_B.b_kstr_n] =
1296 tmp_0[cartesian_waypoints_planner_B.b_kstr_n];
1297 }
1298
1299 cartesian_waypoints_planner_B.b_bool_n = false;
1300 if (a->size[1] == 9) {
1301 cartesian_waypoints_planner_B.b_kstr_n = 1;
1302 do {
1303 exitg1 = 0;
1304 if (cartesian_waypoints_planner_B.b_kstr_n - 1 < 9) {
1305 cartesian_waypoints_planner_B.loop_ub_h =
1306 cartesian_waypoints_planner_B.b_kstr_n - 1;
1307 if (a->data[cartesian_waypoints_planner_B.loop_ub_h] !=
1308 cartesian_waypoints_planner_B.b_p[cartesian_waypoints_planner_B.loop_ub_h])
1309 {
1310 exitg1 = 1;
1311 } else {
1312 cartesian_waypoints_planner_B.b_kstr_n++;
1313 }
1314 } else {
1315 cartesian_waypoints_planner_B.b_bool_n = true;
1316 exitg1 = 1;
1317 }
1318 } while (exitg1 == 0);
1319 }
1320
1321 if (cartesian_waypoints_planner_B.b_bool_n) {
1322 guard1 = true;
1323 } else {
1324 ax[0] = (rtNaN);
1325 ax[1] = (rtNaN);
1326 ax[2] = (rtNaN);
1327 }
1328 }
1329
1330 if (guard1) {
1331 ax[0] = obj->JointAxisInternal[0];
1332 ax[1] = obj->JointAxisInternal[1];
1333 ax[2] = obj->JointAxisInternal[2];
1334 }
1335
1336 cartesian_waypoi_emxFree_char_T(&a);
1337}
1338
1339static void cartesian_waypoints_planner_cat(real_T varargin_1, real_T varargin_2,
1340 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
1341 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9])
1342{
1343 y[0] = varargin_1;
1344 y[1] = varargin_2;
1345 y[2] = varargin_3;
1346 y[3] = varargin_4;
1347 y[4] = varargin_5;
1348 y[5] = varargin_6;
1349 y[6] = varargin_7;
1350 y[7] = varargin_8;
1351 y[8] = varargin_9;
1352}
1353
1354static void rigidBodyJoint_transformBodyT_e(const
1355 c_rigidBodyJoint_cartesian__e_T *obj, const real_T q_data[], const int32_T
1356 *q_size, real_T T[16])
1357{
1358 emxArray_char_T_cartesian_way_T *switch_expression;
1359 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
1360
1361 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1362
1363 int32_T exitg1;
1364 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
1365 cartesian_waypoints_planner_B.b_kstr_f = switch_expression->size[0] *
1366 switch_expression->size[1];
1367 switch_expression->size[0] = 1;
1368 switch_expression->size[1] = obj->Type->size[1];
1369 cartes_emxEnsureCapacity_char_T(switch_expression,
1370 cartesian_waypoints_planner_B.b_kstr_f);
1371 cartesian_waypoints_planner_B.loop_ub_e = obj->Type->size[0] * obj->Type->
1372 size[1] - 1;
1373 for (cartesian_waypoints_planner_B.b_kstr_f = 0;
1374 cartesian_waypoints_planner_B.b_kstr_f <=
1375 cartesian_waypoints_planner_B.loop_ub_e;
1376 cartesian_waypoints_planner_B.b_kstr_f++) {
1377 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_f] = obj->
1378 Type->data[cartesian_waypoints_planner_B.b_kstr_f];
1379 }
1380
1381 for (cartesian_waypoints_planner_B.b_kstr_f = 0;
1382 cartesian_waypoints_planner_B.b_kstr_f < 5;
1383 cartesian_waypoints_planner_B.b_kstr_f++) {
1384 cartesian_waypoints_planner_B.b_il[cartesian_waypoints_planner_B.b_kstr_f] =
1385 tmp[cartesian_waypoints_planner_B.b_kstr_f];
1386 }
1387
1388 cartesian_waypoints_planner_B.b_bool_h = false;
1389 if (switch_expression->size[1] == 5) {
1390 cartesian_waypoints_planner_B.b_kstr_f = 1;
1391 do {
1392 exitg1 = 0;
1393 if (cartesian_waypoints_planner_B.b_kstr_f - 1 < 5) {
1394 cartesian_waypoints_planner_B.loop_ub_e =
1395 cartesian_waypoints_planner_B.b_kstr_f - 1;
1396 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_e] !=
1397 cartesian_waypoints_planner_B.b_il[cartesian_waypoints_planner_B.loop_ub_e])
1398 {
1399 exitg1 = 1;
1400 } else {
1401 cartesian_waypoints_planner_B.b_kstr_f++;
1402 }
1403 } else {
1404 cartesian_waypoints_planner_B.b_bool_h = true;
1405 exitg1 = 1;
1406 }
1407 } while (exitg1 == 0);
1408 }
1409
1410 if (cartesian_waypoints_planner_B.b_bool_h) {
1411 cartesian_waypoints_planner_B.b_kstr_f = 0;
1412 } else {
1413 for (cartesian_waypoints_planner_B.b_kstr_f = 0;
1414 cartesian_waypoints_planner_B.b_kstr_f < 8;
1415 cartesian_waypoints_planner_B.b_kstr_f++) {
1416 cartesian_waypoints_planner_B.b_j[cartesian_waypoints_planner_B.b_kstr_f] =
1417 tmp_0[cartesian_waypoints_planner_B.b_kstr_f];
1418 }
1419
1420 cartesian_waypoints_planner_B.b_bool_h = false;
1421 if (switch_expression->size[1] == 8) {
1422 cartesian_waypoints_planner_B.b_kstr_f = 1;
1423 do {
1424 exitg1 = 0;
1425 if (cartesian_waypoints_planner_B.b_kstr_f - 1 < 8) {
1426 cartesian_waypoints_planner_B.loop_ub_e =
1427 cartesian_waypoints_planner_B.b_kstr_f - 1;
1428 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_e]
1429 !=
1430 cartesian_waypoints_planner_B.b_j[cartesian_waypoints_planner_B.loop_ub_e])
1431 {
1432 exitg1 = 1;
1433 } else {
1434 cartesian_waypoints_planner_B.b_kstr_f++;
1435 }
1436 } else {
1437 cartesian_waypoints_planner_B.b_bool_h = true;
1438 exitg1 = 1;
1439 }
1440 } while (exitg1 == 0);
1441 }
1442
1443 if (cartesian_waypoints_planner_B.b_bool_h) {
1444 cartesian_waypoints_planner_B.b_kstr_f = 1;
1445 } else {
1446 cartesian_waypoints_planner_B.b_kstr_f = -1;
1447 }
1448 }
1449
1450 cartesian_waypoi_emxFree_char_T(&switch_expression);
1451 switch (cartesian_waypoints_planner_B.b_kstr_f) {
1452 case 0:
1453 memset(&cartesian_waypoints_planner_B.TJ[0], 0, sizeof(real_T) << 4U);
1454 cartesian_waypoints_planner_B.TJ[0] = 1.0;
1455 cartesian_waypoints_planner_B.TJ[5] = 1.0;
1456 cartesian_waypoints_planner_B.TJ[10] = 1.0;
1457 cartesian_waypoints_planner_B.TJ[15] = 1.0;
1458 break;
1459
1460 case 1:
1461 rigidBodyJoint_get_JointAxis_e(obj, cartesian_waypoints_planner_B.v_m);
1462 cartesian_waypoints_planner_B.loop_ub_e = (*q_size != 0) - 1;
1463 cartesian_waypoints_planner_B.result_data[0] =
1464 cartesian_waypoints_planner_B.v_m[0];
1465 cartesian_waypoints_planner_B.result_data[1] =
1466 cartesian_waypoints_planner_B.v_m[1];
1467 cartesian_waypoints_planner_B.result_data[2] =
1468 cartesian_waypoints_planner_B.v_m[2];
1469 for (cartesian_waypoints_planner_B.b_kstr_f = 0;
1470 cartesian_waypoints_planner_B.b_kstr_f <=
1471 cartesian_waypoints_planner_B.loop_ub_e;
1472 cartesian_waypoints_planner_B.b_kstr_f++) {
1473 cartesian_waypoints_planner_B.result_data[3] = q_data[0];
1474 }
1475
1476 cartesian_waypoints_planner_B.cth = 1.0 / sqrt
1477 ((cartesian_waypoints_planner_B.result_data[0] *
1478 cartesian_waypoints_planner_B.result_data[0] +
1479 cartesian_waypoints_planner_B.result_data[1] *
1480 cartesian_waypoints_planner_B.result_data[1]) +
1481 cartesian_waypoints_planner_B.result_data[2] *
1482 cartesian_waypoints_planner_B.result_data[2]);
1483 cartesian_waypoints_planner_B.v_m[0] =
1484 cartesian_waypoints_planner_B.result_data[0] *
1485 cartesian_waypoints_planner_B.cth;
1486 cartesian_waypoints_planner_B.v_m[1] =
1487 cartesian_waypoints_planner_B.result_data[1] *
1488 cartesian_waypoints_planner_B.cth;
1489 cartesian_waypoints_planner_B.v_m[2] =
1490 cartesian_waypoints_planner_B.result_data[2] *
1491 cartesian_waypoints_planner_B.cth;
1492 cartesian_waypoints_planner_B.cth = cos
1493 (cartesian_waypoints_planner_B.result_data[3]);
1494 cartesian_waypoints_planner_B.sth = sin
1495 (cartesian_waypoints_planner_B.result_data[3]);
1496 cartesian_waypoints_planner_B.tempR_tmp = cartesian_waypoints_planner_B.v_m
1497 [1] * cartesian_waypoints_planner_B.v_m[0] * (1.0 -
1498 cartesian_waypoints_planner_B.cth);
1499 cartesian_waypoints_planner_B.tempR_tmp_c =
1500 cartesian_waypoints_planner_B.v_m[2] * cartesian_waypoints_planner_B.sth;
1501 cartesian_waypoints_planner_B.tempR_tmp_i =
1502 cartesian_waypoints_planner_B.v_m[2] * cartesian_waypoints_planner_B.v_m[0]
1503 * (1.0 - cartesian_waypoints_planner_B.cth);
1504 cartesian_waypoints_planner_B.tempR_tmp_d =
1505 cartesian_waypoints_planner_B.v_m[1] * cartesian_waypoints_planner_B.sth;
1506 cartesian_waypoints_planner_B.tempR_tmp_g =
1507 cartesian_waypoints_planner_B.v_m[2] * cartesian_waypoints_planner_B.v_m[1]
1508 * (1.0 - cartesian_waypoints_planner_B.cth);
1509 cartesian_waypoints_planner_B.sth *= cartesian_waypoints_planner_B.v_m[0];
1510 cartesian_waypoints_planner_cat(cartesian_waypoints_planner_B.v_m[0] *
1511 cartesian_waypoints_planner_B.v_m[0] * (1.0 -
1512 cartesian_waypoints_planner_B.cth) + cartesian_waypoints_planner_B.cth,
1513 cartesian_waypoints_planner_B.tempR_tmp -
1514 cartesian_waypoints_planner_B.tempR_tmp_c,
1515 cartesian_waypoints_planner_B.tempR_tmp_i +
1516 cartesian_waypoints_planner_B.tempR_tmp_d,
1517 cartesian_waypoints_planner_B.tempR_tmp +
1518 cartesian_waypoints_planner_B.tempR_tmp_c,
1519 cartesian_waypoints_planner_B.v_m[1] * cartesian_waypoints_planner_B.v_m[1]
1520 * (1.0 - cartesian_waypoints_planner_B.cth) +
1521 cartesian_waypoints_planner_B.cth,
1522 cartesian_waypoints_planner_B.tempR_tmp_g -
1523 cartesian_waypoints_planner_B.sth,
1524 cartesian_waypoints_planner_B.tempR_tmp_i -
1525 cartesian_waypoints_planner_B.tempR_tmp_d,
1526 cartesian_waypoints_planner_B.tempR_tmp_g +
1527 cartesian_waypoints_planner_B.sth, cartesian_waypoints_planner_B.v_m[2] *
1528 cartesian_waypoints_planner_B.v_m[2] * (1.0 -
1529 cartesian_waypoints_planner_B.cth) + cartesian_waypoints_planner_B.cth,
1530 cartesian_waypoints_planner_B.tempR_o);
1531 for (cartesian_waypoints_planner_B.b_kstr_f = 0;
1532 cartesian_waypoints_planner_B.b_kstr_f < 3;
1533 cartesian_waypoints_planner_B.b_kstr_f++) {
1534 cartesian_waypoints_planner_B.loop_ub_e =
1535 cartesian_waypoints_planner_B.b_kstr_f + 1;
1536 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.loop_ub_e
1537 - 1] = cartesian_waypoints_planner_B.tempR_o
1538 [(cartesian_waypoints_planner_B.loop_ub_e - 1) * 3];
1539 cartesian_waypoints_planner_B.loop_ub_e =
1540 cartesian_waypoints_planner_B.b_kstr_f + 1;
1541 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.loop_ub_e
1542 + 2] = cartesian_waypoints_planner_B.tempR_o
1543 [(cartesian_waypoints_planner_B.loop_ub_e - 1) * 3 + 1];
1544 cartesian_waypoints_planner_B.loop_ub_e =
1545 cartesian_waypoints_planner_B.b_kstr_f + 1;
1546 cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.loop_ub_e
1547 + 5] = cartesian_waypoints_planner_B.tempR_o
1548 [(cartesian_waypoints_planner_B.loop_ub_e - 1) * 3 + 2];
1549 }
1550
1551 memset(&cartesian_waypoints_planner_B.TJ[0], 0, sizeof(real_T) << 4U);
1552 for (cartesian_waypoints_planner_B.b_kstr_f = 0;
1553 cartesian_waypoints_planner_B.b_kstr_f < 3;
1554 cartesian_waypoints_planner_B.b_kstr_f++) {
1555 cartesian_waypoints_planner_B.loop_ub_e =
1556 cartesian_waypoints_planner_B.b_kstr_f << 2;
1557 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_e] =
1558 cartesian_waypoints_planner_B.R_l[3 *
1559 cartesian_waypoints_planner_B.b_kstr_f];
1560 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_e +
1561 1] = cartesian_waypoints_planner_B.R_l[3 *
1562 cartesian_waypoints_planner_B.b_kstr_f + 1];
1563 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_e +
1564 2] = cartesian_waypoints_planner_B.R_l[3 *
1565 cartesian_waypoints_planner_B.b_kstr_f + 2];
1566 }
1567
1568 cartesian_waypoints_planner_B.TJ[15] = 1.0;
1569 break;
1570
1571 default:
1572 rigidBodyJoint_get_JointAxis_e(obj, cartesian_waypoints_planner_B.v_m);
1573 memset(&cartesian_waypoints_planner_B.tempR_o[0], 0, 9U * sizeof(real_T));
1574 cartesian_waypoints_planner_B.tempR_o[0] = 1.0;
1575 cartesian_waypoints_planner_B.tempR_o[4] = 1.0;
1576 cartesian_waypoints_planner_B.tempR_o[8] = 1.0;
1577 for (cartesian_waypoints_planner_B.b_kstr_f = 0;
1578 cartesian_waypoints_planner_B.b_kstr_f < 3;
1579 cartesian_waypoints_planner_B.b_kstr_f++) {
1580 cartesian_waypoints_planner_B.loop_ub_e =
1581 cartesian_waypoints_planner_B.b_kstr_f << 2;
1582 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_e] =
1583 cartesian_waypoints_planner_B.tempR_o[3 *
1584 cartesian_waypoints_planner_B.b_kstr_f];
1585 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_e +
1586 1] = cartesian_waypoints_planner_B.tempR_o[3 *
1587 cartesian_waypoints_planner_B.b_kstr_f + 1];
1588 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_e +
1589 2] = cartesian_waypoints_planner_B.tempR_o[3 *
1590 cartesian_waypoints_planner_B.b_kstr_f + 2];
1591 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.b_kstr_f +
1592 12] =
1593 cartesian_waypoints_planner_B.v_m[cartesian_waypoints_planner_B.b_kstr_f]
1594 * q_data[0];
1595 }
1596
1597 cartesian_waypoints_planner_B.TJ[3] = 0.0;
1598 cartesian_waypoints_planner_B.TJ[7] = 0.0;
1599 cartesian_waypoints_planner_B.TJ[11] = 0.0;
1600 cartesian_waypoints_planner_B.TJ[15] = 1.0;
1601 break;
1602 }
1603
1604 for (cartesian_waypoints_planner_B.b_kstr_f = 0;
1605 cartesian_waypoints_planner_B.b_kstr_f < 16;
1606 cartesian_waypoints_planner_B.b_kstr_f++) {
1607 cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_f] =
1608 obj->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_f];
1609 }
1610
1611 for (cartesian_waypoints_planner_B.b_kstr_f = 0;
1612 cartesian_waypoints_planner_B.b_kstr_f < 16;
1613 cartesian_waypoints_planner_B.b_kstr_f++) {
1614 cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.b_kstr_f] =
1615 obj->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_f];
1616 }
1617
1618 for (cartesian_waypoints_planner_B.b_kstr_f = 0;
1619 cartesian_waypoints_planner_B.b_kstr_f < 4;
1620 cartesian_waypoints_planner_B.b_kstr_f++) {
1621 for (cartesian_waypoints_planner_B.loop_ub_e = 0;
1622 cartesian_waypoints_planner_B.loop_ub_e < 4;
1623 cartesian_waypoints_planner_B.loop_ub_e++) {
1624 cartesian_waypoints_planner_B.a_tmp_tmp =
1625 cartesian_waypoints_planner_B.loop_ub_e << 2;
1626 cartesian_waypoints_planner_B.a_tmp =
1627 cartesian_waypoints_planner_B.b_kstr_f +
1628 cartesian_waypoints_planner_B.a_tmp_tmp;
1629 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] =
1630 0.0;
1631 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=
1632 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp]
1633 * cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_f];
1634 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=
1635 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp
1636 + 1] *
1637 cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_f +
1638 4];
1639 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=
1640 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp
1641 + 2] *
1642 cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_f +
1643 8];
1644 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=
1645 cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp
1646 + 3] *
1647 cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_f +
1648 12];
1649 }
1650
1651 for (cartesian_waypoints_planner_B.loop_ub_e = 0;
1652 cartesian_waypoints_planner_B.loop_ub_e < 4;
1653 cartesian_waypoints_planner_B.loop_ub_e++) {
1654 cartesian_waypoints_planner_B.a_tmp_tmp =
1655 cartesian_waypoints_planner_B.loop_ub_e << 2;
1656 cartesian_waypoints_planner_B.a_tmp =
1657 cartesian_waypoints_planner_B.b_kstr_f +
1658 cartesian_waypoints_planner_B.a_tmp_tmp;
1659 T[cartesian_waypoints_planner_B.a_tmp] = 0.0;
1660 T[cartesian_waypoints_planner_B.a_tmp] +=
1661 cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp]
1662 * cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_f];
1663 T[cartesian_waypoints_planner_B.a_tmp] +=
1664 cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp
1665 + 1] *
1666 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_f
1667 + 4];
1668 T[cartesian_waypoints_planner_B.a_tmp] +=
1669 cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp
1670 + 2] *
1671 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_f
1672 + 8];
1673 T[cartesian_waypoints_planner_B.a_tmp] +=
1674 cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp
1675 + 3] *
1676 cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_f
1677 + 12];
1678 }
1679 }
1680}
1681
1682static void rigidBodyJoint_transformBodyToP(const
1683 c_rigidBodyJoint_cartesian__e_T *obj, real_T T[16])
1684{
1685 emxArray_char_T_cartesian_way_T *switch_expression;
1686 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
1687
1688 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1689
1690 int32_T exitg1;
1691 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
1692 cartesian_waypoints_planner_B.b_kstr_j = switch_expression->size[0] *
1693 switch_expression->size[1];
1694 switch_expression->size[0] = 1;
1695 switch_expression->size[1] = obj->Type->size[1];
1696 cartes_emxEnsureCapacity_char_T(switch_expression,
1697 cartesian_waypoints_planner_B.b_kstr_j);
1698 cartesian_waypoints_planner_B.loop_ub_hc = obj->Type->size[0] * obj->
1699 Type->size[1] - 1;
1700 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
1701 cartesian_waypoints_planner_B.b_kstr_j <=
1702 cartesian_waypoints_planner_B.loop_ub_hc;
1703 cartesian_waypoints_planner_B.b_kstr_j++) {
1704 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_j] = obj->
1705 Type->data[cartesian_waypoints_planner_B.b_kstr_j];
1706 }
1707
1708 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
1709 cartesian_waypoints_planner_B.b_kstr_j < 5;
1710 cartesian_waypoints_planner_B.b_kstr_j++) {
1711 cartesian_waypoints_planner_B.b_f[cartesian_waypoints_planner_B.b_kstr_j] =
1712 tmp[cartesian_waypoints_planner_B.b_kstr_j];
1713 }
1714
1715 cartesian_waypoints_planner_B.b_bool_a = false;
1716 if (switch_expression->size[1] == 5) {
1717 cartesian_waypoints_planner_B.b_kstr_j = 1;
1718 do {
1719 exitg1 = 0;
1720 if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 5) {
1721 cartesian_waypoints_planner_B.loop_ub_hc =
1722 cartesian_waypoints_planner_B.b_kstr_j - 1;
1723 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_hc] !=
1724 cartesian_waypoints_planner_B.b_f[cartesian_waypoints_planner_B.loop_ub_hc])
1725 {
1726 exitg1 = 1;
1727 } else {
1728 cartesian_waypoints_planner_B.b_kstr_j++;
1729 }
1730 } else {
1731 cartesian_waypoints_planner_B.b_bool_a = true;
1732 exitg1 = 1;
1733 }
1734 } while (exitg1 == 0);
1735 }
1736
1737 if (cartesian_waypoints_planner_B.b_bool_a) {
1738 cartesian_waypoints_planner_B.b_kstr_j = 0;
1739 } else {
1740 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
1741 cartesian_waypoints_planner_B.b_kstr_j < 8;
1742 cartesian_waypoints_planner_B.b_kstr_j++) {
1743 cartesian_waypoints_planner_B.b_mv[cartesian_waypoints_planner_B.b_kstr_j]
1744 = tmp_0[cartesian_waypoints_planner_B.b_kstr_j];
1745 }
1746
1747 cartesian_waypoints_planner_B.b_bool_a = false;
1748 if (switch_expression->size[1] == 8) {
1749 cartesian_waypoints_planner_B.b_kstr_j = 1;
1750 do {
1751 exitg1 = 0;
1752 if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 8) {
1753 cartesian_waypoints_planner_B.loop_ub_hc =
1754 cartesian_waypoints_planner_B.b_kstr_j - 1;
1755 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_hc]
1756 !=
1757 cartesian_waypoints_planner_B.b_mv[cartesian_waypoints_planner_B.loop_ub_hc])
1758 {
1759 exitg1 = 1;
1760 } else {
1761 cartesian_waypoints_planner_B.b_kstr_j++;
1762 }
1763 } else {
1764 cartesian_waypoints_planner_B.b_bool_a = true;
1765 exitg1 = 1;
1766 }
1767 } while (exitg1 == 0);
1768 }
1769
1770 if (cartesian_waypoints_planner_B.b_bool_a) {
1771 cartesian_waypoints_planner_B.b_kstr_j = 1;
1772 } else {
1773 cartesian_waypoints_planner_B.b_kstr_j = -1;
1774 }
1775 }
1776
1777 cartesian_waypoi_emxFree_char_T(&switch_expression);
1778 switch (cartesian_waypoints_planner_B.b_kstr_j) {
1779 case 0:
1780 memset(&cartesian_waypoints_planner_B.TJ_l[0], 0, sizeof(real_T) << 4U);
1781 cartesian_waypoints_planner_B.TJ_l[0] = 1.0;
1782 cartesian_waypoints_planner_B.TJ_l[5] = 1.0;
1783 cartesian_waypoints_planner_B.TJ_l[10] = 1.0;
1784 cartesian_waypoints_planner_B.TJ_l[15] = 1.0;
1785 break;
1786
1787 case 1:
1788 rigidBodyJoint_get_JointAxis_e(obj, cartesian_waypoints_planner_B.v_c);
1789 cartesian_waypoints_planner_B.axang_idx_0 =
1790 cartesian_waypoints_planner_B.v_c[0];
1791 cartesian_waypoints_planner_B.axang_idx_1 =
1792 cartesian_waypoints_planner_B.v_c[1];
1793 cartesian_waypoints_planner_B.axang_idx_2 =
1794 cartesian_waypoints_planner_B.v_c[2];
1795 cartesian_waypoints_planner_B.b_bd = 1.0 / sqrt
1796 ((cartesian_waypoints_planner_B.axang_idx_0 *
1797 cartesian_waypoints_planner_B.axang_idx_0 +
1798 cartesian_waypoints_planner_B.axang_idx_1 *
1799 cartesian_waypoints_planner_B.axang_idx_1) +
1800 cartesian_waypoints_planner_B.axang_idx_2 *
1801 cartesian_waypoints_planner_B.axang_idx_2);
1802 cartesian_waypoints_planner_B.v_c[0] =
1803 cartesian_waypoints_planner_B.axang_idx_0 *
1804 cartesian_waypoints_planner_B.b_bd;
1805 cartesian_waypoints_planner_B.v_c[1] =
1806 cartesian_waypoints_planner_B.axang_idx_1 *
1807 cartesian_waypoints_planner_B.b_bd;
1808 cartesian_waypoints_planner_B.v_c[2] =
1809 cartesian_waypoints_planner_B.axang_idx_2 *
1810 cartesian_waypoints_planner_B.b_bd;
1811 cartesian_waypoints_planner_B.axang_idx_0 =
1812 cartesian_waypoints_planner_B.v_c[1] * cartesian_waypoints_planner_B.v_c[0]
1813 * 0.0;
1814 cartesian_waypoints_planner_B.axang_idx_1 =
1815 cartesian_waypoints_planner_B.v_c[2] * cartesian_waypoints_planner_B.v_c[0]
1816 * 0.0;
1817 cartesian_waypoints_planner_B.axang_idx_2 =
1818 cartesian_waypoints_planner_B.v_c[2] * cartesian_waypoints_planner_B.v_c[1]
1819 * 0.0;
1820 cartesian_waypoints_planner_cat(cartesian_waypoints_planner_B.v_c[0] *
1821 cartesian_waypoints_planner_B.v_c[0] * 0.0 + 1.0,
1822 cartesian_waypoints_planner_B.axang_idx_0 -
1823 cartesian_waypoints_planner_B.v_c[2] * 0.0,
1824 cartesian_waypoints_planner_B.axang_idx_1 +
1825 cartesian_waypoints_planner_B.v_c[1] * 0.0,
1826 cartesian_waypoints_planner_B.axang_idx_0 +
1827 cartesian_waypoints_planner_B.v_c[2] * 0.0,
1828 cartesian_waypoints_planner_B.v_c[1] * cartesian_waypoints_planner_B.v_c[1]
1829 * 0.0 + 1.0, cartesian_waypoints_planner_B.axang_idx_2 -
1830 cartesian_waypoints_planner_B.v_c[0] * 0.0,
1831 cartesian_waypoints_planner_B.axang_idx_1 -
1832 cartesian_waypoints_planner_B.v_c[1] * 0.0,
1833 cartesian_waypoints_planner_B.axang_idx_2 +
1834 cartesian_waypoints_planner_B.v_c[0] * 0.0,
1835 cartesian_waypoints_planner_B.v_c[2] * cartesian_waypoints_planner_B.v_c[2]
1836 * 0.0 + 1.0, cartesian_waypoints_planner_B.tempR_i);
1837 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
1838 cartesian_waypoints_planner_B.b_kstr_j < 3;
1839 cartesian_waypoints_planner_B.b_kstr_j++) {
1840 cartesian_waypoints_planner_B.loop_ub_hc =
1841 cartesian_waypoints_planner_B.b_kstr_j + 1;
1842 cartesian_waypoints_planner_B.R_o[cartesian_waypoints_planner_B.loop_ub_hc
1843 - 1] = cartesian_waypoints_planner_B.tempR_i
1844 [(cartesian_waypoints_planner_B.loop_ub_hc - 1) * 3];
1845 cartesian_waypoints_planner_B.loop_ub_hc =
1846 cartesian_waypoints_planner_B.b_kstr_j + 1;
1847 cartesian_waypoints_planner_B.R_o[cartesian_waypoints_planner_B.loop_ub_hc
1848 + 2] = cartesian_waypoints_planner_B.tempR_i
1849 [(cartesian_waypoints_planner_B.loop_ub_hc - 1) * 3 + 1];
1850 cartesian_waypoints_planner_B.loop_ub_hc =
1851 cartesian_waypoints_planner_B.b_kstr_j + 1;
1852 cartesian_waypoints_planner_B.R_o[cartesian_waypoints_planner_B.loop_ub_hc
1853 + 5] = cartesian_waypoints_planner_B.tempR_i
1854 [(cartesian_waypoints_planner_B.loop_ub_hc - 1) * 3 + 2];
1855 }
1856
1857 memset(&cartesian_waypoints_planner_B.TJ_l[0], 0, sizeof(real_T) << 4U);
1858 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
1859 cartesian_waypoints_planner_B.b_kstr_j < 3;
1860 cartesian_waypoints_planner_B.b_kstr_j++) {
1861 cartesian_waypoints_planner_B.loop_ub_hc =
1862 cartesian_waypoints_planner_B.b_kstr_j << 2;
1863 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_hc]
1864 = cartesian_waypoints_planner_B.R_o[3 *
1865 cartesian_waypoints_planner_B.b_kstr_j];
1866 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_hc
1867 + 1] = cartesian_waypoints_planner_B.R_o[3 *
1868 cartesian_waypoints_planner_B.b_kstr_j + 1];
1869 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_hc
1870 + 2] = cartesian_waypoints_planner_B.R_o[3 *
1871 cartesian_waypoints_planner_B.b_kstr_j + 2];
1872 }
1873
1874 cartesian_waypoints_planner_B.TJ_l[15] = 1.0;
1875 break;
1876
1877 default:
1878 rigidBodyJoint_get_JointAxis_e(obj, cartesian_waypoints_planner_B.v_c);
1879 memset(&cartesian_waypoints_planner_B.tempR_i[0], 0, 9U * sizeof(real_T));
1880 cartesian_waypoints_planner_B.tempR_i[0] = 1.0;
1881 cartesian_waypoints_planner_B.tempR_i[4] = 1.0;
1882 cartesian_waypoints_planner_B.tempR_i[8] = 1.0;
1883 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
1884 cartesian_waypoints_planner_B.b_kstr_j < 3;
1885 cartesian_waypoints_planner_B.b_kstr_j++) {
1886 cartesian_waypoints_planner_B.loop_ub_hc =
1887 cartesian_waypoints_planner_B.b_kstr_j << 2;
1888 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_hc]
1889 = cartesian_waypoints_planner_B.tempR_i[3 *
1890 cartesian_waypoints_planner_B.b_kstr_j];
1891 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_hc
1892 + 1] = cartesian_waypoints_planner_B.tempR_i[3 *
1893 cartesian_waypoints_planner_B.b_kstr_j + 1];
1894 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_hc
1895 + 2] = cartesian_waypoints_planner_B.tempR_i[3 *
1896 cartesian_waypoints_planner_B.b_kstr_j + 2];
1897 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.b_kstr_j
1898 + 12] =
1899 cartesian_waypoints_planner_B.v_c[cartesian_waypoints_planner_B.b_kstr_j]
1900 * 0.0;
1901 }
1902
1903 cartesian_waypoints_planner_B.TJ_l[3] = 0.0;
1904 cartesian_waypoints_planner_B.TJ_l[7] = 0.0;
1905 cartesian_waypoints_planner_B.TJ_l[11] = 0.0;
1906 cartesian_waypoints_planner_B.TJ_l[15] = 1.0;
1907 break;
1908 }
1909
1910 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
1911 cartesian_waypoints_planner_B.b_kstr_j < 16;
1912 cartesian_waypoints_planner_B.b_kstr_j++) {
1913 cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_j] =
1914 obj->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_j];
1915 }
1916
1917 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
1918 cartesian_waypoints_planner_B.b_kstr_j < 16;
1919 cartesian_waypoints_planner_B.b_kstr_j++) {
1920 cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.b_kstr_j] =
1921 obj->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_j];
1922 }
1923
1924 for (cartesian_waypoints_planner_B.b_kstr_j = 0;
1925 cartesian_waypoints_planner_B.b_kstr_j < 4;
1926 cartesian_waypoints_planner_B.b_kstr_j++) {
1927 for (cartesian_waypoints_planner_B.loop_ub_hc = 0;
1928 cartesian_waypoints_planner_B.loop_ub_hc < 4;
1929 cartesian_waypoints_planner_B.loop_ub_hc++) {
1930 cartesian_waypoints_planner_B.a_tmp_tmp_d =
1931 cartesian_waypoints_planner_B.loop_ub_hc << 2;
1932 cartesian_waypoints_planner_B.a_tmp_f =
1933 cartesian_waypoints_planner_B.b_kstr_j +
1934 cartesian_waypoints_planner_B.a_tmp_tmp_d;
1935 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_f] =
1936 0.0;
1937 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_f] +=
1938 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_d]
1939 * cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_j];
1940 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_f] +=
1941 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_d
1942 + 1] *
1943 cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_j
1944 + 4];
1945 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_f] +=
1946 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_d
1947 + 2] *
1948 cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_j
1949 + 8];
1950 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_f] +=
1951 cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_d
1952 + 3] *
1953 cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_j
1954 + 12];
1955 }
1956
1957 for (cartesian_waypoints_planner_B.loop_ub_hc = 0;
1958 cartesian_waypoints_planner_B.loop_ub_hc < 4;
1959 cartesian_waypoints_planner_B.loop_ub_hc++) {
1960 cartesian_waypoints_planner_B.a_tmp_tmp_d =
1961 cartesian_waypoints_planner_B.loop_ub_hc << 2;
1962 cartesian_waypoints_planner_B.a_tmp_f =
1963 cartesian_waypoints_planner_B.b_kstr_j +
1964 cartesian_waypoints_planner_B.a_tmp_tmp_d;
1965 T[cartesian_waypoints_planner_B.a_tmp_f] = 0.0;
1966 T[cartesian_waypoints_planner_B.a_tmp_f] +=
1967 cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_d]
1968 * cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_j];
1969 T[cartesian_waypoints_planner_B.a_tmp_f] +=
1970 cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_d
1971 + 1] *
1972 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_j
1973 + 4];
1974 T[cartesian_waypoints_planner_B.a_tmp_f] +=
1975 cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_d
1976 + 2] *
1977 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_j
1978 + 8];
1979 T[cartesian_waypoints_planner_B.a_tmp_f] +=
1980 cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_d
1981 + 3] *
1982 cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_j
1983 + 12];
1984 }
1985 }
1986}
1987
1988static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
1989 const real_T qv[6], const emxArray_char_T_cartesian_way_T *body1Name, real_T
1990 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_way_T *Jac)
1991{
1992 w_robotics_manip_internal_Rig_T *body1;
1993 w_robotics_manip_internal_Rig_T *body2;
1994 emxArray_real_T_cartesian_way_T *kinematicPathIndices;
1995 c_rigidBodyJoint_cartesian__e_T *joint;
1996 emxArray_char_T_cartesian_way_T *body2Name;
1997 emxArray_real_T_cartesian_way_T *ancestorIndices1;
1998 emxArray_real_T_cartesian_way_T *ancestorIndices2;
1999 emxArray_real_T_cartesian_way_T *y;
2000 emxArray_real_T_cartesian_way_T *b;
2001 w_robotics_manip_internal_Rig_T *body;
2002 emxArray_char_T_cartesian_way_T *bname;
2003 emxArray_real_T_cartesian_way_T *ancestorIndices1_0;
2004 emxArray_real_T_cartesian_way_T *ancestorIndices2_0;
2005 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
2006
2007 boolean_T exitg1;
2008 int32_T exitg2;
2009 cartesian_waypoi_emxInit_char_T(&body2Name, 2);
2010 cartesian_waypoints_planner_B.b_kstr = body2Name->size[0] * body2Name->size[1];
2011 body2Name->size[0] = 1;
2012 body2Name->size[1] = obj->Base.NameInternal->size[1];
2013 cartes_emxEnsureCapacity_char_T(body2Name,
2014 cartesian_waypoints_planner_B.b_kstr);
2015 cartesian_waypoints_planner_B.loop_ub_p = obj->Base.NameInternal->size[0] *
2016 obj->Base.NameInternal->size[1] - 1;
2017 for (cartesian_waypoints_planner_B.b_kstr = 0;
2018 cartesian_waypoints_planner_B.b_kstr <=
2019 cartesian_waypoints_planner_B.loop_ub_p;
2020 cartesian_waypoints_planner_B.b_kstr++) {
2021 body2Name->data[cartesian_waypoints_planner_B.b_kstr] =
2022 obj->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr];
2023 }
2024
2025 cartesian_waypoi_emxInit_char_T(&bname, 2);
2026 cartesian_waypoints_planner_B.bid1 = -1.0;
2027 cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];
2028 bname->size[0] = 1;
2029 bname->size[1] = obj->Base.NameInternal->size[1];
2030 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr);
2031 cartesian_waypoints_planner_B.loop_ub_p = obj->Base.NameInternal->size[0] *
2032 obj->Base.NameInternal->size[1] - 1;
2033 for (cartesian_waypoints_planner_B.b_kstr = 0;
2034 cartesian_waypoints_planner_B.b_kstr <=
2035 cartesian_waypoints_planner_B.loop_ub_p;
2036 cartesian_waypoints_planner_B.b_kstr++) {
2037 bname->data[cartesian_waypoints_planner_B.b_kstr] = obj->
2038 Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr];
2039 }
2040
2041 if (cartesian_waypoints_plan_strcmp(bname, body1Name)) {
2042 cartesian_waypoints_planner_B.bid1 = 0.0;
2043 } else {
2044 cartesian_waypoints_planner_B.qidx_idx_1 = obj->NumBodies;
2045 cartesian_waypoints_planner_B.b_i_hw = 0;
2046 exitg1 = false;
2047 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_hw <=
2048 static_cast<int32_T>
2049 (cartesian_waypoints_planner_B.qidx_idx_1) - 1)) {
2050 body1 = obj->Bodies[cartesian_waypoints_planner_B.b_i_hw];
2051 cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];
2052 bname->size[0] = 1;
2053 bname->size[1] = body1->NameInternal->size[1];
2054 cartes_emxEnsureCapacity_char_T(bname,
2055 cartesian_waypoints_planner_B.b_kstr);
2056 cartesian_waypoints_planner_B.loop_ub_p = body1->NameInternal->size[0] *
2057 body1->NameInternal->size[1] - 1;
2058 for (cartesian_waypoints_planner_B.b_kstr = 0;
2059 cartesian_waypoints_planner_B.b_kstr <=
2060 cartesian_waypoints_planner_B.loop_ub_p;
2061 cartesian_waypoints_planner_B.b_kstr++) {
2062 bname->data[cartesian_waypoints_planner_B.b_kstr] = body1->
2063 NameInternal->data[cartesian_waypoints_planner_B.b_kstr];
2064 }
2065
2066 if (cartesian_waypoints_plan_strcmp(bname, body1Name)) {
2067 cartesian_waypoints_planner_B.bid1 = static_cast<real_T>
2068 (cartesian_waypoints_planner_B.b_i_hw) + 1.0;
2069 exitg1 = true;
2070 } else {
2071 cartesian_waypoints_planner_B.b_i_hw++;
2072 }
2073 }
2074 }
2075
2076 cartesian_waypoints_planner_B.bid2 = -1.0;
2077 cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];
2078 bname->size[0] = 1;
2079 bname->size[1] = obj->Base.NameInternal->size[1];
2080 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr);
2081 cartesian_waypoints_planner_B.loop_ub_p = obj->Base.NameInternal->size[0] *
2082 obj->Base.NameInternal->size[1] - 1;
2083 for (cartesian_waypoints_planner_B.b_kstr = 0;
2084 cartesian_waypoints_planner_B.b_kstr <=
2085 cartesian_waypoints_planner_B.loop_ub_p;
2086 cartesian_waypoints_planner_B.b_kstr++) {
2087 bname->data[cartesian_waypoints_planner_B.b_kstr] = obj->
2088 Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr];
2089 }
2090
2091 if (cartesian_waypoints_plan_strcmp(bname, body2Name)) {
2092 cartesian_waypoints_planner_B.bid2 = 0.0;
2093 } else {
2094 cartesian_waypoints_planner_B.qidx_idx_1 = obj->NumBodies;
2095 cartesian_waypoints_planner_B.b_i_hw = 0;
2096 exitg1 = false;
2097 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_hw <=
2098 static_cast<int32_T>
2099 (cartesian_waypoints_planner_B.qidx_idx_1) - 1)) {
2100 body1 = obj->Bodies[cartesian_waypoints_planner_B.b_i_hw];
2101 cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];
2102 bname->size[0] = 1;
2103 bname->size[1] = body1->NameInternal->size[1];
2104 cartes_emxEnsureCapacity_char_T(bname,
2105 cartesian_waypoints_planner_B.b_kstr);
2106 cartesian_waypoints_planner_B.loop_ub_p = body1->NameInternal->size[0] *
2107 body1->NameInternal->size[1] - 1;
2108 for (cartesian_waypoints_planner_B.b_kstr = 0;
2109 cartesian_waypoints_planner_B.b_kstr <=
2110 cartesian_waypoints_planner_B.loop_ub_p;
2111 cartesian_waypoints_planner_B.b_kstr++) {
2112 bname->data[cartesian_waypoints_planner_B.b_kstr] = body1->
2113 NameInternal->data[cartesian_waypoints_planner_B.b_kstr];
2114 }
2115
2116 if (cartesian_waypoints_plan_strcmp(bname, body2Name)) {
2117 cartesian_waypoints_planner_B.bid2 = static_cast<real_T>
2118 (cartesian_waypoints_planner_B.b_i_hw) + 1.0;
2119 exitg1 = true;
2120 } else {
2121 cartesian_waypoints_planner_B.b_i_hw++;
2122 }
2123 }
2124 }
2125
2126 cartesian_waypoi_emxFree_char_T(&bname);
2127 if (cartesian_waypoints_planner_B.bid1 == 0.0) {
2128 body1 = &obj->Base;
2129 } else {
2130 body1 = obj->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.bid1)
2131 - 1];
2132 }
2133
2134 if (cartesian_waypoints_planner_B.bid2 == 0.0) {
2135 body2 = &obj->Base;
2136 } else {
2137 body2 = obj->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.bid2)
2138 - 1];
2139 }
2140
2141 cartesian_waypoi_emxInit_real_T(&ancestorIndices1, 2);
2142 body = body1;
2143 cartesian_waypoints_planner_B.b_kstr = ancestorIndices1->size[0] *
2144 ancestorIndices1->size[1];
2145 ancestorIndices1->size[0] = 1;
2146 ancestorIndices1->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
2147 cartes_emxEnsureCapacity_real_T(ancestorIndices1,
2148 cartesian_waypoints_planner_B.b_kstr);
2149 cartesian_waypoints_planner_B.loop_ub_p = static_cast<int32_T>(obj->NumBodies
2150 + 1.0) - 1;
2151 for (cartesian_waypoints_planner_B.b_kstr = 0;
2152 cartesian_waypoints_planner_B.b_kstr <=
2153 cartesian_waypoints_planner_B.loop_ub_p;
2154 cartesian_waypoints_planner_B.b_kstr++) {
2155 ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr] = 0.0;
2156 }
2157
2158 cartesian_waypoints_planner_B.bid1 = 2.0;
2159 ancestorIndices1->data[0] = body1->Index;
2160 while (body->ParentIndex > 0.0) {
2161 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
2162 ancestorIndices1->data[static_cast<int32_T>
2163 (cartesian_waypoints_planner_B.bid1) - 1] = body->Index;
2164 cartesian_waypoints_planner_B.bid1++;
2165 }
2166
2167 if (body->Index > 0.0) {
2168 ancestorIndices1->data[static_cast<int32_T>
2169 (cartesian_waypoints_planner_B.bid1) - 1] = body->ParentIndex;
2170 cartesian_waypoints_planner_B.bid1++;
2171 }
2172
2173 cartesian_waypoi_emxInit_real_T(&ancestorIndices1_0, 2);
2174 cartesian_waypoints_planner_B.loop_ub_p = static_cast<int32_T>
2175 (cartesian_waypoints_planner_B.bid1 - 1.0);
2176 cartesian_waypoints_planner_B.b_kstr = ancestorIndices1_0->size[0] *
2177 ancestorIndices1_0->size[1];
2178 ancestorIndices1_0->size[0] = 1;
2179 ancestorIndices1_0->size[1] = cartesian_waypoints_planner_B.loop_ub_p;
2180 cartes_emxEnsureCapacity_real_T(ancestorIndices1_0,
2181 cartesian_waypoints_planner_B.b_kstr);
2182 for (cartesian_waypoints_planner_B.b_kstr = 0;
2183 cartesian_waypoints_planner_B.b_kstr <
2184 cartesian_waypoints_planner_B.loop_ub_p;
2185 cartesian_waypoints_planner_B.b_kstr++) {
2186 ancestorIndices1_0->data[cartesian_waypoints_planner_B.b_kstr] =
2187 ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr];
2188 }
2189
2190 cartesian_waypoints_planner_B.b_kstr = ancestorIndices1->size[0] *
2191 ancestorIndices1->size[1];
2192 ancestorIndices1->size[0] = 1;
2193 ancestorIndices1->size[1] = ancestorIndices1_0->size[1];
2194 cartes_emxEnsureCapacity_real_T(ancestorIndices1,
2195 cartesian_waypoints_planner_B.b_kstr);
2196 cartesian_waypoints_planner_B.loop_ub_p = ancestorIndices1_0->size[0] *
2197 ancestorIndices1_0->size[1];
2198 for (cartesian_waypoints_planner_B.b_kstr = 0;
2199 cartesian_waypoints_planner_B.b_kstr <
2200 cartesian_waypoints_planner_B.loop_ub_p;
2201 cartesian_waypoints_planner_B.b_kstr++) {
2202 ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr] =
2203 ancestorIndices1_0->data[cartesian_waypoints_planner_B.b_kstr];
2204 }
2205
2206 cartesian_waypoi_emxFree_real_T(&ancestorIndices1_0);
2207 cartesian_waypoi_emxInit_real_T(&ancestorIndices2, 2);
2208 body = body2;
2209 cartesian_waypoints_planner_B.b_kstr = ancestorIndices2->size[0] *
2210 ancestorIndices2->size[1];
2211 ancestorIndices2->size[0] = 1;
2212 ancestorIndices2->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
2213 cartes_emxEnsureCapacity_real_T(ancestorIndices2,
2214 cartesian_waypoints_planner_B.b_kstr);
2215 cartesian_waypoints_planner_B.loop_ub_p = static_cast<int32_T>(obj->NumBodies
2216 + 1.0) - 1;
2217 for (cartesian_waypoints_planner_B.b_kstr = 0;
2218 cartesian_waypoints_planner_B.b_kstr <=
2219 cartesian_waypoints_planner_B.loop_ub_p;
2220 cartesian_waypoints_planner_B.b_kstr++) {
2221 ancestorIndices2->data[cartesian_waypoints_planner_B.b_kstr] = 0.0;
2222 }
2223
2224 cartesian_waypoints_planner_B.bid1 = 2.0;
2225 ancestorIndices2->data[0] = body2->Index;
2226 while (body->ParentIndex > 0.0) {
2227 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
2228 ancestorIndices2->data[static_cast<int32_T>
2229 (cartesian_waypoints_planner_B.bid1) - 1] = body->Index;
2230 cartesian_waypoints_planner_B.bid1++;
2231 }
2232
2233 if (body->Index > 0.0) {
2234 ancestorIndices2->data[static_cast<int32_T>
2235 (cartesian_waypoints_planner_B.bid1) - 1] = body->ParentIndex;
2236 cartesian_waypoints_planner_B.bid1++;
2237 }
2238
2239 cartesian_waypoi_emxInit_real_T(&ancestorIndices2_0, 2);
2240 cartesian_waypoints_planner_B.loop_ub_p = static_cast<int32_T>
2241 (cartesian_waypoints_planner_B.bid1 - 1.0);
2242 cartesian_waypoints_planner_B.b_kstr = ancestorIndices2_0->size[0] *
2243 ancestorIndices2_0->size[1];
2244 ancestorIndices2_0->size[0] = 1;
2245 ancestorIndices2_0->size[1] = cartesian_waypoints_planner_B.loop_ub_p;
2246 cartes_emxEnsureCapacity_real_T(ancestorIndices2_0,
2247 cartesian_waypoints_planner_B.b_kstr);
2248 for (cartesian_waypoints_planner_B.b_kstr = 0;
2249 cartesian_waypoints_planner_B.b_kstr <
2250 cartesian_waypoints_planner_B.loop_ub_p;
2251 cartesian_waypoints_planner_B.b_kstr++) {
2252 ancestorIndices2_0->data[cartesian_waypoints_planner_B.b_kstr] =
2253 ancestorIndices2->data[cartesian_waypoints_planner_B.b_kstr];
2254 }
2255
2256 cartesian_waypoints_planner_B.b_kstr = ancestorIndices2->size[0] *
2257 ancestorIndices2->size[1];
2258 ancestorIndices2->size[0] = 1;
2259 ancestorIndices2->size[1] = ancestorIndices2_0->size[1];
2260 cartes_emxEnsureCapacity_real_T(ancestorIndices2,
2261 cartesian_waypoints_planner_B.b_kstr);
2262 cartesian_waypoints_planner_B.loop_ub_p = ancestorIndices2_0->size[0] *
2263 ancestorIndices2_0->size[1];
2264 for (cartesian_waypoints_planner_B.b_kstr = 0;
2265 cartesian_waypoints_planner_B.b_kstr <
2266 cartesian_waypoints_planner_B.loop_ub_p;
2267 cartesian_waypoints_planner_B.b_kstr++) {
2268 ancestorIndices2->data[cartesian_waypoints_planner_B.b_kstr] =
2269 ancestorIndices2_0->data[cartesian_waypoints_planner_B.b_kstr];
2270 }
2271
2272 cartesian_waypoi_emxFree_real_T(&ancestorIndices2_0);
2273 cartesian_waypoints_planner_B.bid1 = ancestorIndices1->size[1];
2274 cartesian_waypoints_planner_B.qidx_idx_1 = ancestorIndices2->size[1];
2275 if (cartesian_waypoints_planner_B.bid1 <
2276 cartesian_waypoints_planner_B.qidx_idx_1) {
2277 cartesian_waypoints_planner_B.qidx_idx_1 =
2278 cartesian_waypoints_planner_B.bid1;
2279 }
2280
2281 cartesian_waypoints_planner_B.bid1 = static_cast<int32_T>
2282 (cartesian_waypoints_planner_B.qidx_idx_1);
2283 cartesian_waypoints_planner_B.b_i_hw = 0;
2284 exitg1 = false;
2285 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_hw <=
2286 static_cast<int32_T>
2287 (cartesian_waypoints_planner_B.qidx_idx_1) - 2)) {
2288 if (ancestorIndices1->data[static_cast<int32_T>(static_cast<real_T>
2289 (ancestorIndices1->size[1]) - (static_cast<real_T>
2290 (cartesian_waypoints_planner_B.b_i_hw) + 1.0)) - 1] !=
2291 ancestorIndices2->data[static_cast<int32_T>(static_cast<real_T>
2292 (ancestorIndices2->size[1]) - (static_cast<real_T>
2293 (cartesian_waypoints_planner_B.b_i_hw) + 1.0)) - 1]) {
2294 cartesian_waypoints_planner_B.bid1 = static_cast<real_T>
2295 (cartesian_waypoints_planner_B.b_i_hw) + 1.0;
2296 exitg1 = true;
2297 } else {
2298 cartesian_waypoints_planner_B.b_i_hw++;
2299 }
2300 }
2301
2302 cartesian_waypoints_planner_B.qidx_idx_1 = static_cast<real_T>
2303 (ancestorIndices1->size[1]) - cartesian_waypoints_planner_B.bid1;
2304 if (1.0 > cartesian_waypoints_planner_B.qidx_idx_1) {
2305 cartesian_waypoints_planner_B.b_i_hw = -1;
2306 } else {
2307 cartesian_waypoints_planner_B.b_i_hw = static_cast<int32_T>
2308 (cartesian_waypoints_planner_B.qidx_idx_1) - 1;
2309 }
2310
2311 cartesian_waypoints_planner_B.qidx_idx_1 = static_cast<real_T>
2312 (ancestorIndices2->size[1]) - cartesian_waypoints_planner_B.bid1;
2313 if (1.0 > cartesian_waypoints_planner_B.qidx_idx_1) {
2314 cartesian_waypoints_planner_B.j_c = 0;
2315 cartesian_waypoints_planner_B.jointSign = 1;
2316 cartesian_waypoints_planner_B.g = -1;
2317 } else {
2318 cartesian_waypoints_planner_B.j_c = static_cast<int32_T>
2319 (cartesian_waypoints_planner_B.qidx_idx_1) - 1;
2320 cartesian_waypoints_planner_B.jointSign = -1;
2321 cartesian_waypoints_planner_B.g = 0;
2322 }
2323
2324 cartesian_waypoi_emxInit_real_T(&kinematicPathIndices, 2);
2325 cartesian_waypoints_planner_B.b_kstr = kinematicPathIndices->size[0] *
2326 kinematicPathIndices->size[1];
2327 kinematicPathIndices->size[0] = 1;
2328 cartesian_waypoints_planner_B.loop_ub_p = div_s32_floor
2329 (cartesian_waypoints_planner_B.g - cartesian_waypoints_planner_B.j_c,
2330 cartesian_waypoints_planner_B.jointSign);
2331 kinematicPathIndices->size[1] = (cartesian_waypoints_planner_B.loop_ub_p +
2332 cartesian_waypoints_planner_B.b_i_hw) + 3;
2333 cartes_emxEnsureCapacity_real_T(kinematicPathIndices,
2334 cartesian_waypoints_planner_B.b_kstr);
2335 for (cartesian_waypoints_planner_B.b_kstr = 0;
2336 cartesian_waypoints_planner_B.b_kstr <=
2337 cartesian_waypoints_planner_B.b_i_hw;
2338 cartesian_waypoints_planner_B.b_kstr++) {
2339 kinematicPathIndices->data[cartesian_waypoints_planner_B.b_kstr] =
2340 ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr];
2341 }
2342
2343 kinematicPathIndices->data[cartesian_waypoints_planner_B.b_i_hw + 1] =
2344 ancestorIndices1->data[static_cast<int32_T>((static_cast<real_T>
2345 (ancestorIndices1->size[1]) - cartesian_waypoints_planner_B.bid1) + 1.0) - 1];
2346 cartesian_waypoi_emxFree_real_T(&ancestorIndices1);
2347 for (cartesian_waypoints_planner_B.b_kstr = 0;
2348 cartesian_waypoints_planner_B.b_kstr <=
2349 cartesian_waypoints_planner_B.loop_ub_p;
2350 cartesian_waypoints_planner_B.b_kstr++) {
2351 kinematicPathIndices->data[(cartesian_waypoints_planner_B.b_kstr +
2352 cartesian_waypoints_planner_B.b_i_hw) + 2] = ancestorIndices2->
2353 data[cartesian_waypoints_planner_B.jointSign *
2354 cartesian_waypoints_planner_B.b_kstr + cartesian_waypoints_planner_B.j_c];
2355 }
2356
2357 cartesian_waypoi_emxFree_real_T(&ancestorIndices2);
2358 memset(&cartesian_waypoints_planner_B.T1[0], 0, sizeof(real_T) << 4U);
2359 cartesian_waypoints_planner_B.T1[0] = 1.0;
2360 cartesian_waypoints_planner_B.T1[5] = 1.0;
2361 cartesian_waypoints_planner_B.T1[10] = 1.0;
2362 cartesian_waypoints_planner_B.T1[15] = 1.0;
2363 cartesian_waypoints_planner_B.b_kstr = Jac->size[0] * Jac->size[1];
2364 Jac->size[0] = 6;
2365 Jac->size[1] = static_cast<int32_T>(obj->PositionNumber);
2366 cartes_emxEnsureCapacity_real_T(Jac, cartesian_waypoints_planner_B.b_kstr);
2367 cartesian_waypoints_planner_B.loop_ub_p = 6 * static_cast<int32_T>
2368 (obj->PositionNumber) - 1;
2369 for (cartesian_waypoints_planner_B.b_kstr = 0;
2370 cartesian_waypoints_planner_B.b_kstr <=
2371 cartesian_waypoints_planner_B.loop_ub_p;
2372 cartesian_waypoints_planner_B.b_kstr++) {
2373 Jac->data[cartesian_waypoints_planner_B.b_kstr] = 0.0;
2374 }
2375
2376 cartesian_waypoints_planner_B.j_c = kinematicPathIndices->size[1] - 2;
2377 cartesian_waypoi_emxInit_real_T(&y, 2);
2378 cartesian_waypoi_emxInit_real_T(&b, 2);
2379 if (0 <= cartesian_waypoints_planner_B.j_c) {
2380 for (cartesian_waypoints_planner_B.b_kstr = 0;
2381 cartesian_waypoints_planner_B.b_kstr < 5;
2382 cartesian_waypoints_planner_B.b_kstr++) {
2383 cartesian_waypoints_planner_B.b_a0[cartesian_waypoints_planner_B.b_kstr] =
2384 tmp[cartesian_waypoints_planner_B.b_kstr];
2385 }
2386 }
2387
2388 for (cartesian_waypoints_planner_B.b_i_hw = 0;
2389 cartesian_waypoints_planner_B.b_i_hw <= cartesian_waypoints_planner_B.j_c;
2390 cartesian_waypoints_planner_B.b_i_hw++) {
2391 if (kinematicPathIndices->data[cartesian_waypoints_planner_B.b_i_hw] != 0.0)
2392 {
2393 body1 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->
2394 data[cartesian_waypoints_planner_B.b_i_hw]) - 1];
2395 } else {
2396 body1 = &obj->Base;
2397 }
2398
2399 cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>
2400 ((static_cast<real_T>(cartesian_waypoints_planner_B.b_i_hw) + 1.0) + 1.0)
2401 - 1;
2402 if (kinematicPathIndices->data[cartesian_waypoints_planner_B.b_kstr] != 0.0)
2403 {
2404 body2 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->
2405 data[cartesian_waypoints_planner_B.b_kstr]) - 1];
2406 } else {
2407 body2 = &obj->Base;
2408 }
2409
2410 cartesian_waypoints_planner_B.nextBodyIsParent = (body2->Index ==
2411 body1->ParentIndex);
2412 if (cartesian_waypoints_planner_B.nextBodyIsParent) {
2413 body2 = body1;
2414 cartesian_waypoints_planner_B.jointSign = 1;
2415 } else {
2416 cartesian_waypoints_planner_B.jointSign = -1;
2417 }
2418
2419 joint = body2->JointInternal;
2420 cartesian_waypoints_planner_B.b_kstr = body2Name->size[0] * body2Name->size
2421 [1];
2422 body2Name->size[0] = 1;
2423 body2Name->size[1] = joint->Type->size[1];
2424 cartes_emxEnsureCapacity_char_T(body2Name,
2425 cartesian_waypoints_planner_B.b_kstr);
2426 cartesian_waypoints_planner_B.loop_ub_p = joint->Type->size[0] * joint->
2427 Type->size[1] - 1;
2428 for (cartesian_waypoints_planner_B.b_kstr = 0;
2429 cartesian_waypoints_planner_B.b_kstr <=
2430 cartesian_waypoints_planner_B.loop_ub_p;
2431 cartesian_waypoints_planner_B.b_kstr++) {
2432 body2Name->data[cartesian_waypoints_planner_B.b_kstr] = joint->Type->
2433 data[cartesian_waypoints_planner_B.b_kstr];
2434 }
2435
2436 cartesian_waypoints_planner_B.b_bool = false;
2437 if (body2Name->size[1] == 5) {
2438 cartesian_waypoints_planner_B.b_kstr = 1;
2439 do {
2440 exitg2 = 0;
2441 if (cartesian_waypoints_planner_B.b_kstr - 1 < 5) {
2442 cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr
2443 - 1;
2444 if (body2Name->data[cartesian_waypoints_planner_B.g] !=
2445 cartesian_waypoints_planner_B.b_a0[cartesian_waypoints_planner_B.g])
2446 {
2447 exitg2 = 1;
2448 } else {
2449 cartesian_waypoints_planner_B.b_kstr++;
2450 }
2451 } else {
2452 cartesian_waypoints_planner_B.b_bool = true;
2453 exitg2 = 1;
2454 }
2455 } while (exitg2 == 0);
2456 }
2457
2458 if (cartesian_waypoints_planner_B.b_bool) {
2459 rigidBodyJoint_transformBodyToP(joint, cartesian_waypoints_planner_B.Tc2p);
2460 } else {
2461 cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>(body2->Index);
2462 cartesian_waypoints_planner_B.bid1 = obj->
2463 PositionDoFMap[cartesian_waypoints_planner_B.b_kstr - 1];
2464 cartesian_waypoints_planner_B.qidx_idx_1 = obj->
2465 PositionDoFMap[cartesian_waypoints_planner_B.b_kstr + 7];
2466 if (cartesian_waypoints_planner_B.bid1 >
2467 cartesian_waypoints_planner_B.qidx_idx_1) {
2468 cartesian_waypoints_planner_B.g = 0;
2469 cartesian_waypoints_planner_B.b_kstr = -1;
2470 } else {
2471 cartesian_waypoints_planner_B.g = static_cast<int32_T>
2472 (cartesian_waypoints_planner_B.bid1) - 1;
2473 cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>
2474 (cartesian_waypoints_planner_B.qidx_idx_1) - 1;
2475 }
2476
2477 cartesian_waypoints_planner_B.loop_ub_p =
2478 cartesian_waypoints_planner_B.b_kstr - cartesian_waypoints_planner_B.g;
2479 cartesian_waypoints_planner_B.qv_size =
2480 cartesian_waypoints_planner_B.loop_ub_p + 1;
2481 for (cartesian_waypoints_planner_B.b_kstr = 0;
2482 cartesian_waypoints_planner_B.b_kstr <=
2483 cartesian_waypoints_planner_B.loop_ub_p;
2484 cartesian_waypoints_planner_B.b_kstr++) {
2485 cartesian_waypoints_planner_B.qv_data[cartesian_waypoints_planner_B.b_kstr]
2486 = qv[cartesian_waypoints_planner_B.g +
2487 cartesian_waypoints_planner_B.b_kstr];
2488 }
2489
2490 rigidBodyJoint_transformBodyT_e(joint,
2491 cartesian_waypoints_planner_B.qv_data,
2492 &cartesian_waypoints_planner_B.qv_size,
2493 cartesian_waypoints_planner_B.Tc2p);
2494 cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>(body2->Index);
2495 cartesian_waypoints_planner_B.bid1 = obj->
2496 VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr - 1];
2497 cartesian_waypoints_planner_B.qidx_idx_1 = obj->
2498 VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr + 7];
2499 if (cartesian_waypoints_planner_B.nextBodyIsParent) {
2500 for (cartesian_waypoints_planner_B.b_kstr = 0;
2501 cartesian_waypoints_planner_B.b_kstr < 16;
2502 cartesian_waypoints_planner_B.b_kstr++) {
2503 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr]
2504 = joint->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr];
2505 }
2506 } else {
2507 for (cartesian_waypoints_planner_B.b_kstr = 0;
2508 cartesian_waypoints_planner_B.b_kstr < 16;
2509 cartesian_waypoints_planner_B.b_kstr++) {
2510 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr]
2511 = joint->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr];
2512 }
2513
2514 for (cartesian_waypoints_planner_B.b_kstr = 0;
2515 cartesian_waypoints_planner_B.b_kstr < 3;
2516 cartesian_waypoints_planner_B.b_kstr++) {
2517 cartesian_waypoints_planner_B.R_a[3 *
2518 cartesian_waypoints_planner_B.b_kstr] =
2519 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr];
2520 cartesian_waypoints_planner_B.R_a[3 *
2521 cartesian_waypoints_planner_B.b_kstr + 1] =
2522 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2523 + 4];
2524 cartesian_waypoints_planner_B.R_a[3 *
2525 cartesian_waypoints_planner_B.b_kstr + 2] =
2526 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2527 + 8];
2528 }
2529
2530 for (cartesian_waypoints_planner_B.b_kstr = 0;
2531 cartesian_waypoints_planner_B.b_kstr < 9;
2532 cartesian_waypoints_planner_B.b_kstr++) {
2533 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr]
2534 =
2535 -cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.b_kstr];
2536 }
2537
2538 for (cartesian_waypoints_planner_B.b_kstr = 0;
2539 cartesian_waypoints_planner_B.b_kstr < 3;
2540 cartesian_waypoints_planner_B.b_kstr++) {
2541 cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr
2542 << 2;
2543 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g] =
2544 cartesian_waypoints_planner_B.R_a[3 *
2545 cartesian_waypoints_planner_B.b_kstr];
2546 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 1] =
2547 cartesian_waypoints_planner_B.R_a[3 *
2548 cartesian_waypoints_planner_B.b_kstr + 1];
2549 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 2] =
2550 cartesian_waypoints_planner_B.R_a[3 *
2551 cartesian_waypoints_planner_B.b_kstr + 2];
2552 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr
2553 + 12] =
2554 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr
2555 + 6] * cartesian_waypoints_planner_B.T1j[14] +
2556 (cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr
2557 + 3] * cartesian_waypoints_planner_B.T1j[13] +
2558 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr]
2559 * cartesian_waypoints_planner_B.T1j[12]);
2560 }
2561
2562 cartesian_waypoints_planner_B.Tj[3] = 0.0;
2563 cartesian_waypoints_planner_B.Tj[7] = 0.0;
2564 cartesian_waypoints_planner_B.Tj[11] = 0.0;
2565 cartesian_waypoints_planner_B.Tj[15] = 1.0;
2566 }
2567
2568 for (cartesian_waypoints_planner_B.b_kstr = 0;
2569 cartesian_waypoints_planner_B.b_kstr < 4;
2570 cartesian_waypoints_planner_B.b_kstr++) {
2571 for (cartesian_waypoints_planner_B.g = 0;
2572 cartesian_waypoints_planner_B.g < 4;
2573 cartesian_waypoints_planner_B.g++) {
2574 cartesian_waypoints_planner_B.loop_ub_p =
2575 cartesian_waypoints_planner_B.g << 2;
2576 cartesian_waypoints_planner_B.n_i =
2577 cartesian_waypoints_planner_B.b_kstr +
2578 cartesian_waypoints_planner_B.loop_ub_p;
2579 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_i] =
2580 0.0;
2581 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_i] +=
2582 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_p]
2583 * cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr];
2584 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_i] +=
2585 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_p
2586 + 1] *
2587 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr
2588 + 4];
2589 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_i] +=
2590 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_p
2591 + 2] *
2592 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr
2593 + 8];
2594 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_i] +=
2595 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_p
2596 + 3] *
2597 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr
2598 + 12];
2599 }
2600 }
2601
2602 for (cartesian_waypoints_planner_B.b_kstr = 0;
2603 cartesian_waypoints_planner_B.b_kstr < 3;
2604 cartesian_waypoints_planner_B.b_kstr++) {
2605 cartesian_waypoints_planner_B.R_a[3 *
2606 cartesian_waypoints_planner_B.b_kstr] =
2607 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr];
2608 cartesian_waypoints_planner_B.R_a[3 *
2609 cartesian_waypoints_planner_B.b_kstr + 1] =
2610 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2611 + 4];
2612 cartesian_waypoints_planner_B.R_a[3 *
2613 cartesian_waypoints_planner_B.b_kstr + 2] =
2614 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2615 + 8];
2616 }
2617
2618 for (cartesian_waypoints_planner_B.b_kstr = 0;
2619 cartesian_waypoints_planner_B.b_kstr < 9;
2620 cartesian_waypoints_planner_B.b_kstr++) {
2621 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr] =
2622 -cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.b_kstr];
2623 }
2624
2625 for (cartesian_waypoints_planner_B.b_kstr = 0;
2626 cartesian_waypoints_planner_B.b_kstr < 3;
2627 cartesian_waypoints_planner_B.b_kstr++) {
2628 cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr <<
2629 2;
2630 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g] =
2631 cartesian_waypoints_planner_B.R_a[3 *
2632 cartesian_waypoints_planner_B.b_kstr];
2633 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 1] =
2634 cartesian_waypoints_planner_B.R_a[3 *
2635 cartesian_waypoints_planner_B.b_kstr + 1];
2636 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 2] =
2637 cartesian_waypoints_planner_B.R_a[3 *
2638 cartesian_waypoints_planner_B.b_kstr + 2];
2639 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr +
2640 12] =
2641 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr
2642 + 6] * cartesian_waypoints_planner_B.T1j[14] +
2643 (cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr
2644 + 3] * cartesian_waypoints_planner_B.T1j[13] +
2645 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr]
2646 * cartesian_waypoints_planner_B.T1j[12]);
2647 }
2648
2649 cartesian_waypoints_planner_B.Tj[3] = 0.0;
2650 cartesian_waypoints_planner_B.Tj[7] = 0.0;
2651 cartesian_waypoints_planner_B.Tj[11] = 0.0;
2652 cartesian_waypoints_planner_B.Tj[15] = 1.0;
2653 cartesian_waypoints_planner_B.R_a[0] = 0.0;
2654 cartesian_waypoints_planner_B.R_a[3] = -cartesian_waypoints_planner_B.Tj
2655 [14];
2656 cartesian_waypoints_planner_B.R_a[6] = cartesian_waypoints_planner_B.Tj[13];
2657 cartesian_waypoints_planner_B.R_a[1] = cartesian_waypoints_planner_B.Tj[14];
2658 cartesian_waypoints_planner_B.R_a[4] = 0.0;
2659 cartesian_waypoints_planner_B.R_a[7] = -cartesian_waypoints_planner_B.Tj
2660 [12];
2661 cartesian_waypoints_planner_B.R_a[2] = -cartesian_waypoints_planner_B.Tj
2662 [13];
2663 cartesian_waypoints_planner_B.R_a[5] = cartesian_waypoints_planner_B.Tj[12];
2664 cartesian_waypoints_planner_B.R_a[8] = 0.0;
2665 for (cartesian_waypoints_planner_B.b_kstr = 0;
2666 cartesian_waypoints_planner_B.b_kstr < 3;
2667 cartesian_waypoints_planner_B.b_kstr++) {
2668 for (cartesian_waypoints_planner_B.g = 0;
2669 cartesian_waypoints_planner_B.g < 3;
2670 cartesian_waypoints_planner_B.g++) {
2671 cartesian_waypoints_planner_B.loop_ub_p =
2672 cartesian_waypoints_planner_B.b_kstr + 3 *
2673 cartesian_waypoints_planner_B.g;
2674 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.loop_ub_p]
2675 = 0.0;
2676 cartesian_waypoints_planner_B.n_i = cartesian_waypoints_planner_B.g <<
2677 2;
2678 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.loop_ub_p]
2679 +=
2680 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.n_i] *
2681 cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.b_kstr];
2682 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.loop_ub_p]
2683 +=
2684 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.n_i +
2685 1] *
2686 cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.b_kstr
2687 + 3];
2688 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.loop_ub_p]
2689 +=
2690 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.n_i +
2691 2] *
2692 cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.b_kstr
2693 + 6];
2694 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 6 *
2695 cartesian_waypoints_planner_B.b_kstr] =
2696 cartesian_waypoints_planner_B.Tj
2697 [(cartesian_waypoints_planner_B.b_kstr << 2) +
2698 cartesian_waypoints_planner_B.g];
2699 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 6 *
2700 (cartesian_waypoints_planner_B.b_kstr + 3)] = 0.0;
2701 }
2702 }
2703
2704 for (cartesian_waypoints_planner_B.b_kstr = 0;
2705 cartesian_waypoints_planner_B.b_kstr < 3;
2706 cartesian_waypoints_planner_B.b_kstr++) {
2707 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr
2708 + 3] = cartesian_waypoints_planner_B.R_i[3 *
2709 cartesian_waypoints_planner_B.b_kstr];
2710 cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr <<
2711 2;
2712 cartesian_waypoints_planner_B.loop_ub_p = 6 *
2713 (cartesian_waypoints_planner_B.b_kstr + 3);
2714 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_p
2715 + 3] =
2716 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g];
2717 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr
2718 + 4] = cartesian_waypoints_planner_B.R_i[3 *
2719 cartesian_waypoints_planner_B.b_kstr + 1];
2720 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_p
2721 + 4] =
2722 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 1];
2723 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr
2724 + 5] = cartesian_waypoints_planner_B.R_i[3 *
2725 cartesian_waypoints_planner_B.b_kstr + 2];
2726 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_p
2727 + 5] =
2728 cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 2];
2729 }
2730
2731 cartesian_waypoints_planner_B.b_kstr = b->size[0] * b->size[1];
2732 b->size[0] = 6;
2733 b->size[1] = joint->MotionSubspace->size[1];
2734 cartes_emxEnsureCapacity_real_T(b, cartesian_waypoints_planner_B.b_kstr);
2735 cartesian_waypoints_planner_B.loop_ub_p = joint->MotionSubspace->size[0] *
2736 joint->MotionSubspace->size[1] - 1;
2737 for (cartesian_waypoints_planner_B.b_kstr = 0;
2738 cartesian_waypoints_planner_B.b_kstr <=
2739 cartesian_waypoints_planner_B.loop_ub_p;
2740 cartesian_waypoints_planner_B.b_kstr++) {
2741 b->data[cartesian_waypoints_planner_B.b_kstr] = joint->
2742 MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr];
2743 }
2744
2745 cartesian_waypoints_planner_B.n_i = b->size[1] - 1;
2746 cartesian_waypoints_planner_B.b_kstr = y->size[0] * y->size[1];
2747 y->size[0] = 6;
2748 y->size[1] = b->size[1];
2749 cartes_emxEnsureCapacity_real_T(y, cartesian_waypoints_planner_B.b_kstr);
2750 for (cartesian_waypoints_planner_B.b_kstr = 0;
2751 cartesian_waypoints_planner_B.b_kstr <=
2752 cartesian_waypoints_planner_B.n_i;
2753 cartesian_waypoints_planner_B.b_kstr++) {
2754 cartesian_waypoints_planner_B.coffset_tmp =
2755 cartesian_waypoints_planner_B.b_kstr * 6 - 1;
2756 for (cartesian_waypoints_planner_B.g = 0;
2757 cartesian_waypoints_planner_B.g < 6;
2758 cartesian_waypoints_planner_B.g++) {
2759 cartesian_waypoints_planner_B.bid2 = 0.0;
2760 for (cartesian_waypoints_planner_B.loop_ub_p = 0;
2761 cartesian_waypoints_planner_B.loop_ub_p < 6;
2762 cartesian_waypoints_planner_B.loop_ub_p++) {
2763 cartesian_waypoints_planner_B.bid2 +=
2764 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_p
2765 * 6 + cartesian_waypoints_planner_B.g] * b->data
2766 [(cartesian_waypoints_planner_B.coffset_tmp +
2767 cartesian_waypoints_planner_B.loop_ub_p) + 1];
2768 }
2769
2770 y->data[(cartesian_waypoints_planner_B.coffset_tmp +
2771 cartesian_waypoints_planner_B.g) + 1] =
2772 cartesian_waypoints_planner_B.bid2;
2773 }
2774 }
2775
2776 if (cartesian_waypoints_planner_B.bid1 >
2777 cartesian_waypoints_planner_B.qidx_idx_1) {
2778 cartesian_waypoints_planner_B.n_i = 0;
2779 } else {
2780 cartesian_waypoints_planner_B.n_i = static_cast<int32_T>
2781 (cartesian_waypoints_planner_B.bid1) - 1;
2782 }
2783
2784 cartesian_waypoints_planner_B.loop_ub_p = y->size[1];
2785 for (cartesian_waypoints_planner_B.b_kstr = 0;
2786 cartesian_waypoints_planner_B.b_kstr <
2787 cartesian_waypoints_planner_B.loop_ub_p;
2788 cartesian_waypoints_planner_B.b_kstr++) {
2789 for (cartesian_waypoints_planner_B.g = 0;
2790 cartesian_waypoints_planner_B.g < 6;
2791 cartesian_waypoints_planner_B.g++) {
2792 Jac->data[cartesian_waypoints_planner_B.g + 6 *
2793 (cartesian_waypoints_planner_B.n_i +
2794 cartesian_waypoints_planner_B.b_kstr)] = y->data[6 *
2795 cartesian_waypoints_planner_B.b_kstr +
2796 cartesian_waypoints_planner_B.g] * static_cast<real_T>
2797 (cartesian_waypoints_planner_B.jointSign);
2798 }
2799 }
2800 }
2801
2802 if (cartesian_waypoints_planner_B.nextBodyIsParent) {
2803 for (cartesian_waypoints_planner_B.b_kstr = 0;
2804 cartesian_waypoints_planner_B.b_kstr < 4;
2805 cartesian_waypoints_planner_B.b_kstr++) {
2806 for (cartesian_waypoints_planner_B.g = 0;
2807 cartesian_waypoints_planner_B.g < 4;
2808 cartesian_waypoints_planner_B.g++) {
2809 cartesian_waypoints_planner_B.loop_ub_p =
2810 cartesian_waypoints_planner_B.g << 2;
2811 cartesian_waypoints_planner_B.jointSign =
2812 cartesian_waypoints_planner_B.b_kstr +
2813 cartesian_waypoints_planner_B.loop_ub_p;
2814 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]
2815 = 0.0;
2816 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]
2817 +=
2818 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_p]
2819 * cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr];
2820 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]
2821 +=
2822 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_p
2823 + 1] *
2824 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr
2825 + 4];
2826 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]
2827 +=
2828 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_p
2829 + 2] *
2830 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr
2831 + 8];
2832 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]
2833 +=
2834 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_p
2835 + 3] *
2836 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr
2837 + 12];
2838 }
2839 }
2840
2841 memcpy(&cartesian_waypoints_planner_B.T1[0],
2842 &cartesian_waypoints_planner_B.T1j[0], sizeof(real_T) << 4U);
2843 } else {
2844 for (cartesian_waypoints_planner_B.b_kstr = 0;
2845 cartesian_waypoints_planner_B.b_kstr < 3;
2846 cartesian_waypoints_planner_B.b_kstr++) {
2847 cartesian_waypoints_planner_B.R_a[3 *
2848 cartesian_waypoints_planner_B.b_kstr] =
2849 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr];
2850 cartesian_waypoints_planner_B.R_a[3 *
2851 cartesian_waypoints_planner_B.b_kstr + 1] =
2852 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr
2853 + 4];
2854 cartesian_waypoints_planner_B.R_a[3 *
2855 cartesian_waypoints_planner_B.b_kstr + 2] =
2856 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr
2857 + 8];
2858 }
2859
2860 for (cartesian_waypoints_planner_B.b_kstr = 0;
2861 cartesian_waypoints_planner_B.b_kstr < 9;
2862 cartesian_waypoints_planner_B.b_kstr++) {
2863 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr] =
2864 -cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.b_kstr];
2865 }
2866
2867 for (cartesian_waypoints_planner_B.b_kstr = 0;
2868 cartesian_waypoints_planner_B.b_kstr < 3;
2869 cartesian_waypoints_planner_B.b_kstr++) {
2870 cartesian_waypoints_planner_B.loop_ub_p =
2871 cartesian_waypoints_planner_B.b_kstr << 2;
2872 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.loop_ub_p]
2873 = cartesian_waypoints_planner_B.R_a[3 *
2874 cartesian_waypoints_planner_B.b_kstr];
2875 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.loop_ub_p
2876 + 1] = cartesian_waypoints_planner_B.R_a[3 *
2877 cartesian_waypoints_planner_B.b_kstr + 1];
2878 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.loop_ub_p
2879 + 2] = cartesian_waypoints_planner_B.R_a[3 *
2880 cartesian_waypoints_planner_B.b_kstr + 2];
2881 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr +
2882 12] =
2883 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr
2884 + 6] * cartesian_waypoints_planner_B.Tc2p[14] +
2885 (cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr
2886 + 3] * cartesian_waypoints_planner_B.Tc2p[13] +
2887 cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr]
2888 * cartesian_waypoints_planner_B.Tc2p[12]);
2889 }
2890
2891 cartesian_waypoints_planner_B.T1j[3] = 0.0;
2892 cartesian_waypoints_planner_B.T1j[7] = 0.0;
2893 cartesian_waypoints_planner_B.T1j[11] = 0.0;
2894 cartesian_waypoints_planner_B.T1j[15] = 1.0;
2895 for (cartesian_waypoints_planner_B.b_kstr = 0;
2896 cartesian_waypoints_planner_B.b_kstr < 4;
2897 cartesian_waypoints_planner_B.b_kstr++) {
2898 for (cartesian_waypoints_planner_B.g = 0;
2899 cartesian_waypoints_planner_B.g < 4;
2900 cartesian_waypoints_planner_B.g++) {
2901 cartesian_waypoints_planner_B.jointSign =
2902 cartesian_waypoints_planner_B.g << 2;
2903 cartesian_waypoints_planner_B.loop_ub_p =
2904 cartesian_waypoints_planner_B.b_kstr +
2905 cartesian_waypoints_planner_B.jointSign;
2906 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_p]
2907 = 0.0;
2908 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_p]
2909 +=
2910 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign]
2911 * cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr];
2912 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_p]
2913 +=
2914 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign
2915 + 1] *
2916 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2917 + 4];
2918 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_p]
2919 +=
2920 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign
2921 + 2] *
2922 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2923 + 8];
2924 cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_p]
2925 +=
2926 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign
2927 + 3] *
2928 cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr
2929 + 12];
2930 }
2931 }
2932
2933 memcpy(&cartesian_waypoints_planner_B.T1[0],
2934 &cartesian_waypoints_planner_B.Tc2p[0], sizeof(real_T) << 4U);
2935 }
2936 }
2937
2938 cartesian_waypoi_emxFree_real_T(&b);
2939 cartesian_waypoi_emxFree_char_T(&body2Name);
2940 cartesian_waypoi_emxFree_real_T(&kinematicPathIndices);
2941 for (cartesian_waypoints_planner_B.b_kstr = 0;
2942 cartesian_waypoints_planner_B.b_kstr < 3;
2943 cartesian_waypoints_planner_B.b_kstr++) {
2944 cartesian_waypoints_planner_B.b_i_hw = cartesian_waypoints_planner_B.b_kstr <<
2945 2;
2946 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr] =
2947 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_hw];
2948 cartesian_waypoints_planner_B.g = 6 * (cartesian_waypoints_planner_B.b_kstr
2949 + 3);
2950 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g] = 0.0;
2951 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 3]
2952 = 0.0;
2953 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 3] =
2954 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_hw];
2955 cartesian_waypoints_planner_B.bid1 =
2956 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_hw + 1];
2957 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 1]
2958 = cartesian_waypoints_planner_B.bid1;
2959 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 1] = 0.0;
2960 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 4]
2961 = 0.0;
2962 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 4] =
2963 cartesian_waypoints_planner_B.bid1;
2964 cartesian_waypoints_planner_B.bid1 =
2965 cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_hw + 2];
2966 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 2]
2967 = cartesian_waypoints_planner_B.bid1;
2968 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 2] = 0.0;
2969 cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 5]
2970 = 0.0;
2971 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 5] =
2972 cartesian_waypoints_planner_B.bid1;
2973 }
2974
2975 cartesian_waypoints_planner_B.n_i = Jac->size[1];
2976 cartesian_waypoints_planner_B.b_kstr = y->size[0] * y->size[1];
2977 y->size[0] = 6;
2978 y->size[1] = Jac->size[1];
2979 cartes_emxEnsureCapacity_real_T(y, cartesian_waypoints_planner_B.b_kstr);
2980 cartesian_waypoints_planner_B.loop_ub_p = Jac->size[0] * Jac->size[1] - 1;
2981 for (cartesian_waypoints_planner_B.b_kstr = 0;
2982 cartesian_waypoints_planner_B.b_kstr <=
2983 cartesian_waypoints_planner_B.loop_ub_p;
2984 cartesian_waypoints_planner_B.b_kstr++) {
2985 y->data[cartesian_waypoints_planner_B.b_kstr] = Jac->
2986 data[cartesian_waypoints_planner_B.b_kstr];
2987 }
2988
2989 cartesian_waypoints_planner_B.b_kstr = Jac->size[0] * Jac->size[1];
2990 Jac->size[0] = 6;
2991 Jac->size[1] = cartesian_waypoints_planner_B.n_i;
2992 cartes_emxEnsureCapacity_real_T(Jac, cartesian_waypoints_planner_B.b_kstr);
2993 for (cartesian_waypoints_planner_B.b_kstr = 0;
2994 cartesian_waypoints_planner_B.b_kstr < cartesian_waypoints_planner_B.n_i;
2995 cartesian_waypoints_planner_B.b_kstr++) {
2996 cartesian_waypoints_planner_B.coffset_tmp =
2997 cartesian_waypoints_planner_B.b_kstr * 6 - 1;
2998 for (cartesian_waypoints_planner_B.b_i_hw = 0;
2999 cartesian_waypoints_planner_B.b_i_hw < 6;
3000 cartesian_waypoints_planner_B.b_i_hw++) {
3001 cartesian_waypoints_planner_B.bid2 = 0.0;
3002 for (cartesian_waypoints_planner_B.loop_ub_p = 0;
3003 cartesian_waypoints_planner_B.loop_ub_p < 6;
3004 cartesian_waypoints_planner_B.loop_ub_p++) {
3005 cartesian_waypoints_planner_B.bid2 +=
3006 cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_p
3007 * 6 + cartesian_waypoints_planner_B.b_i_hw] * y->data
3008 [(cartesian_waypoints_planner_B.coffset_tmp +
3009 cartesian_waypoints_planner_B.loop_ub_p) + 1];
3010 }
3011
3012 Jac->data[(cartesian_waypoints_planner_B.coffset_tmp +
3013 cartesian_waypoints_planner_B.b_i_hw) + 1] =
3014 cartesian_waypoints_planner_B.bid2;
3015 }
3016 }
3017
3018 cartesian_waypoi_emxFree_real_T(&y);
3019 T_size[0] = 4;
3020 T_size[1] = 4;
3021 memcpy(&T_data[0], &cartesian_waypoints_planner_B.T1[0], sizeof(real_T) << 4U);
3022}
3023
3024real_T rt_hypotd_snf(real_T u0, real_T u1)
3025{
3026 real_T y;
3027 real_T a;
3028 a = fabs(u0);
3029 y = fabs(u1);
3030 if (a < y) {
3031 a /= y;
3032 y *= sqrt(a * a + 1.0);
3033 } else if (a > y) {
3034 y /= a;
3035 y = sqrt(y * y + 1.0) * a;
3036 } else {
3037 if (!rtIsNaN(y)) {
3038 y = a * 1.4142135623730951;
3039 }
3040 }
3041
3042 return y;
3043}
3044
3045static creal_T cartesian_waypoints_planne_sqrt(const creal_T x)
3046{
3047 creal_T b_x;
3048 real_T absxr;
3049 real_T absxi;
3050 if (x.im == 0.0) {
3051 if (x.re < 0.0) {
3052 absxr = 0.0;
3053 absxi = sqrt(-x.re);
3054 } else {
3055 absxr = sqrt(x.re);
3056 absxi = 0.0;
3057 }
3058 } else if (x.re == 0.0) {
3059 if (x.im < 0.0) {
3060 absxr = sqrt(-x.im / 2.0);
3061 absxi = -absxr;
3062 } else {
3063 absxr = sqrt(x.im / 2.0);
3064 absxi = absxr;
3065 }
3066 } else if (rtIsNaN(x.re)) {
3067 absxr = x.re;
3068 absxi = x.re;
3069 } else if (rtIsNaN(x.im)) {
3070 absxr = x.im;
3071 absxi = x.im;
3072 } else if (rtIsInf(x.im)) {
3073 absxr = fabs(x.im);
3074 absxi = x.im;
3075 } else if (rtIsInf(x.re)) {
3076 if (x.re < 0.0) {
3077 absxr = 0.0;
3078 absxi = x.im * -x.re;
3079 } else {
3080 absxr = x.re;
3081 absxi = 0.0;
3082 }
3083 } else {
3084 absxr = fabs(x.re);
3085 absxi = fabs(x.im);
3086 if ((absxr > 4.4942328371557893E+307) || (absxi > 4.4942328371557893E+307))
3087 {
3088 absxr *= 0.5;
3089 absxi *= 0.5;
3090 absxi = rt_hypotd_snf(absxr, absxi);
3091 if (absxi > absxr) {
3092 absxr = sqrt(absxr / absxi + 1.0) * sqrt(absxi);
3093 } else {
3094 absxr = sqrt(absxi) * 1.4142135623730951;
3095 }
3096 } else {
3097 absxr = sqrt((rt_hypotd_snf(absxr, absxi) + absxr) * 0.5);
3098 }
3099
3100 if (x.re > 0.0) {
3101 absxi = x.im / absxr * 0.5;
3102 } else {
3103 if (x.im < 0.0) {
3104 absxi = -absxr;
3105 } else {
3106 absxi = absxr;
3107 }
3108
3109 absxr = x.im / absxi * 0.5;
3110 }
3111 }
3112
3113 b_x.re = absxr;
3114 b_x.im = absxi;
3115 return b_x;
3116}
3117
3118real_T rt_atan2d_snf(real_T u0, real_T u1)
3119{
3120 real_T y;
3121 int32_T u0_0;
3122 int32_T u1_0;
3123 if (rtIsNaN(u0) || rtIsNaN(u1)) {
3124 y = (rtNaN);
3125 } else if (rtIsInf(u0) && rtIsInf(u1)) {
3126 if (u0 > 0.0) {
3127 u0_0 = 1;
3128 } else {
3129 u0_0 = -1;
3130 }
3131
3132 if (u1 > 0.0) {
3133 u1_0 = 1;
3134 } else {
3135 u1_0 = -1;
3136 }
3137
3138 y = atan2(static_cast<real_T>(u0_0), static_cast<real_T>(u1_0));
3139 } else if (u1 == 0.0) {
3140 if (u0 > 0.0) {
3141 y = RT_PI / 2.0;
3142 } else if (u0 < 0.0) {
3143 y = -(RT_PI / 2.0);
3144 } else {
3145 y = 0.0;
3146 }
3147 } else {
3148 y = atan2(u0, u1);
3149 }
3150
3151 return y;
3152}
3153
3154static real_T cartesian_waypoints_plann_xnrm2(int32_T n, const real_T x[9],
3155 int32_T ix0)
3156{
3157 real_T y;
3158 real_T scale;
3159 int32_T kend;
3160 real_T absxk;
3161 real_T t;
3162 int32_T k;
3163 y = 0.0;
3164 scale = 3.3121686421112381E-170;
3165 kend = ix0 + n;
3166 for (k = ix0; k < kend; k++) {
3167 absxk = fabs(x[k - 1]);
3168 if (absxk > scale) {
3169 t = scale / absxk;
3170 y = y * t * t + 1.0;
3171 scale = absxk;
3172 } else {
3173 t = absxk / scale;
3174 y += t * t;
3175 }
3176 }
3177
3178 return scale * sqrt(y);
3179}
3180
3181static real_T cartesian_waypoints_plann_xdotc(int32_T n, const real_T x[9],
3182 int32_T ix0, const real_T y[9], int32_T iy0)
3183{
3184 real_T d;
3185 int32_T ix;
3186 int32_T iy;
3187 int32_T k;
3188 d = 0.0;
3189 ix = ix0 - 1;
3190 iy = iy0 - 1;
3191 for (k = 0; k < n; k++) {
3192 d += x[ix] * y[iy];
3193 ix++;
3194 iy++;
3195 }
3196
3197 return d;
3198}
3199
3200static void cartesian_waypoints_plann_xaxpy(int32_T n, real_T a, int32_T ix0,
3201 const real_T y[9], int32_T iy0, real_T b_y[9])
3202{
3203 int32_T ix;
3204 int32_T iy;
3205 int32_T k;
3206 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
3207 if (!(a == 0.0)) {
3208 ix = ix0;
3209 iy = iy0 - 1;
3210 for (k = 0; k < n; k++) {
3211 b_y[iy] += b_y[ix - 1] * a;
3212 ix++;
3213 iy++;
3214 }
3215 }
3216}
3217
3218static real_T cartesian_waypoints_pla_xnrm2_e(const real_T x[3], int32_T ix0)
3219{
3220 real_T y;
3221 real_T scale;
3222 real_T absxk;
3223 real_T t;
3224 int32_T k;
3225 y = 0.0;
3226 scale = 3.3121686421112381E-170;
3227 for (k = ix0; k <= ix0 + 1; k++) {
3228 absxk = fabs(x[k - 1]);
3229 if (absxk > scale) {
3230 t = scale / absxk;
3231 y = y * t * t + 1.0;
3232 scale = absxk;
3233 } else {
3234 t = absxk / scale;
3235 y += t * t;
3236 }
3237 }
3238
3239 return scale * sqrt(y);
3240}
3241
3242static void cartesian_waypoints_p_xaxpy_evq(int32_T n, real_T a, const real_T x
3243 [9], int32_T ix0, real_T y[3], int32_T iy0)
3244{
3245 int32_T ix;
3246 int32_T iy;
3247 int32_T k;
3248 if (!(a == 0.0)) {
3249 ix = ix0;
3250 iy = iy0 - 1;
3251 for (k = 0; k < n; k++) {
3252 y[iy] += x[ix - 1] * a;
3253 ix++;
3254 iy++;
3255 }
3256 }
3257}
3258
3259static void cartesian_waypoints_pl_xaxpy_ev(int32_T n, real_T a, const real_T x
3260 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9])
3261{
3262 int32_T ix;
3263 int32_T iy;
3264 int32_T k;
3265 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
3266 if (!(a == 0.0)) {
3267 ix = ix0;
3268 iy = iy0 - 1;
3269 for (k = 0; k < n; k++) {
3270 b_y[iy] += x[ix - 1] * a;
3271 ix++;
3272 iy++;
3273 }
3274 }
3275}
3276
3277static void cartesian_waypoints_plann_xswap(const real_T x[9], int32_T ix0,
3278 int32_T iy0, real_T b_x[9])
3279{
3280 int32_T ix;
3281 int32_T iy;
3282 real_T temp;
3283 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
3284 ix = ix0 - 1;
3285 iy = iy0 - 1;
3286 temp = b_x[ix];
3287 b_x[ix] = b_x[iy];
3288 b_x[iy] = temp;
3289 ix++;
3290 iy++;
3291 temp = b_x[ix];
3292 b_x[ix] = b_x[iy];
3293 b_x[iy] = temp;
3294 ix++;
3295 iy++;
3296 temp = b_x[ix];
3297 b_x[ix] = b_x[iy];
3298 b_x[iy] = temp;
3299}
3300
3301static void cartesian_waypoints_plann_xrotg(real_T a, real_T b, real_T *b_a,
3302 real_T *b_b, real_T *c, real_T *s)
3303{
3304 cartesian_waypoints_planner_B.roe = b;
3305 cartesian_waypoints_planner_B.absa = fabs(a);
3306 cartesian_waypoints_planner_B.absb = fabs(b);
3307 if (cartesian_waypoints_planner_B.absa > cartesian_waypoints_planner_B.absb) {
3308 cartesian_waypoints_planner_B.roe = a;
3309 }
3310
3311 cartesian_waypoints_planner_B.scale = cartesian_waypoints_planner_B.absa +
3312 cartesian_waypoints_planner_B.absb;
3313 if (cartesian_waypoints_planner_B.scale == 0.0) {
3314 *s = 0.0;
3315 *c = 1.0;
3316 *b_a = 0.0;
3317 *b_b = 0.0;
3318 } else {
3319 cartesian_waypoints_planner_B.ads = cartesian_waypoints_planner_B.absa /
3320 cartesian_waypoints_planner_B.scale;
3321 cartesian_waypoints_planner_B.bds = cartesian_waypoints_planner_B.absb /
3322 cartesian_waypoints_planner_B.scale;
3323 *b_a = sqrt(cartesian_waypoints_planner_B.ads *
3324 cartesian_waypoints_planner_B.ads +
3325 cartesian_waypoints_planner_B.bds *
3326 cartesian_waypoints_planner_B.bds) *
3327 cartesian_waypoints_planner_B.scale;
3328 if (cartesian_waypoints_planner_B.roe < 0.0) {
3329 *b_a = -*b_a;
3330 }
3331
3332 *c = a / *b_a;
3333 *s = b / *b_a;
3334 if (cartesian_waypoints_planner_B.absa > cartesian_waypoints_planner_B.absb)
3335 {
3336 *b_b = *s;
3337 } else if (*c != 0.0) {
3338 *b_b = 1.0 / *c;
3339 } else {
3340 *b_b = 1.0;
3341 }
3342 }
3343}
3344
3345static void cartesian_waypoints_planne_xrot(const real_T x[9], int32_T ix0,
3346 int32_T iy0, real_T c, real_T s, real_T b_x[9])
3347{
3348 int32_T ix;
3349 int32_T iy;
3350 real_T temp;
3351 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
3352 ix = ix0 - 1;
3353 iy = iy0 - 1;
3354 temp = c * b_x[ix] + s * b_x[iy];
3355 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3356 b_x[ix] = temp;
3357 iy++;
3358 ix++;
3359 temp = c * b_x[ix] + s * b_x[iy];
3360 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3361 b_x[ix] = temp;
3362 iy++;
3363 ix++;
3364 temp = c * b_x[ix] + s * b_x[iy];
3365 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3366 b_x[ix] = temp;
3367}
3368
3369static void cartesian_waypoints_planner_svd(const real_T A[9], real_T U[9],
3370 real_T s[3], real_T V[9])
3371{
3372 int32_T qq;
3373 boolean_T apply_transform;
3374 int32_T qjj;
3375 int32_T m;
3376 int32_T kase;
3377 int32_T c_q;
3378 int32_T d_k;
3379 boolean_T exitg1;
3380 cartesian_waypoints_planner_B.e_h[0] = 0.0;
3381 cartesian_waypoints_planner_B.work[0] = 0.0;
3382 cartesian_waypoints_planner_B.e_h[1] = 0.0;
3383 cartesian_waypoints_planner_B.work[1] = 0.0;
3384 cartesian_waypoints_planner_B.e_h[2] = 0.0;
3385 cartesian_waypoints_planner_B.work[2] = 0.0;
3386 for (m = 0; m < 9; m++) {
3387 cartesian_waypoints_planner_B.A[m] = A[m];
3388 U[m] = 0.0;
3389 V[m] = 0.0;
3390 }
3391
3392 apply_transform = false;
3393 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_plann_xnrm2(3,
3394 cartesian_waypoints_planner_B.A, 1);
3395 if (cartesian_waypoints_planner_B.nrm > 0.0) {
3396 apply_transform = true;
3397 if (cartesian_waypoints_planner_B.A[0] < 0.0) {
3398 cartesian_waypoints_planner_B.s[0] = -cartesian_waypoints_planner_B.nrm;
3399 } else {
3400 cartesian_waypoints_planner_B.s[0] = cartesian_waypoints_planner_B.nrm;
3401 }
3402
3403 if (fabs(cartesian_waypoints_planner_B.s[0]) >= 1.0020841800044864E-292) {
3404 cartesian_waypoints_planner_B.nrm = 1.0 / cartesian_waypoints_planner_B.s
3405 [0];
3406 for (qq = 1; qq < 4; qq++) {
3407 cartesian_waypoints_planner_B.A[qq - 1] *=
3408 cartesian_waypoints_planner_B.nrm;
3409 }
3410 } else {
3411 for (qq = 1; qq < 4; qq++) {
3412 cartesian_waypoints_planner_B.A[qq - 1] /=
3413 cartesian_waypoints_planner_B.s[0];
3414 }
3415 }
3416
3417 cartesian_waypoints_planner_B.A[0]++;
3418 cartesian_waypoints_planner_B.s[0] = -cartesian_waypoints_planner_B.s[0];
3419 } else {
3420 cartesian_waypoints_planner_B.s[0] = 0.0;
3421 }
3422
3423 for (m = 2; m < 4; m++) {
3424 qjj = (m - 1) * 3 + 1;
3425 if (apply_transform) {
3426 memcpy(&cartesian_waypoints_planner_B.A_g[0],
3427 &cartesian_waypoints_planner_B.A[0], 9U * sizeof(real_T));
3428 cartesian_waypoints_plann_xaxpy(3, -(cartesian_waypoints_plann_xdotc(3,
3429 cartesian_waypoints_planner_B.A, 1, cartesian_waypoints_planner_B.A, qjj)
3430 / cartesian_waypoints_planner_B.A[0]), 1,
3431 cartesian_waypoints_planner_B.A_g, qjj, cartesian_waypoints_planner_B.A);
3432 }
3433
3434 cartesian_waypoints_planner_B.e_h[m - 1] =
3435 cartesian_waypoints_planner_B.A[qjj - 1];
3436 }
3437
3438 for (m = 1; m < 4; m++) {
3439 U[m - 1] = cartesian_waypoints_planner_B.A[m - 1];
3440 }
3441
3442 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_pla_xnrm2_e
3443 (cartesian_waypoints_planner_B.e_h, 2);
3444 if (cartesian_waypoints_planner_B.nrm == 0.0) {
3445 cartesian_waypoints_planner_B.e_h[0] = 0.0;
3446 } else {
3447 if (cartesian_waypoints_planner_B.e_h[1] < 0.0) {
3448 cartesian_waypoints_planner_B.rt = -cartesian_waypoints_planner_B.nrm;
3449 cartesian_waypoints_planner_B.e_h[0] = -cartesian_waypoints_planner_B.nrm;
3450 } else {
3451 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.nrm;
3452 cartesian_waypoints_planner_B.e_h[0] = cartesian_waypoints_planner_B.nrm;
3453 }
3454
3455 if (fabs(cartesian_waypoints_planner_B.rt) >= 1.0020841800044864E-292) {
3456 cartesian_waypoints_planner_B.nrm = 1.0 / cartesian_waypoints_planner_B.rt;
3457 for (qq = 2; qq < 4; qq++) {
3458 cartesian_waypoints_planner_B.e_h[qq - 1] *=
3459 cartesian_waypoints_planner_B.nrm;
3460 }
3461 } else {
3462 for (qq = 2; qq < 4; qq++) {
3463 cartesian_waypoints_planner_B.e_h[qq - 1] /=
3464 cartesian_waypoints_planner_B.rt;
3465 }
3466 }
3467
3468 cartesian_waypoints_planner_B.e_h[1]++;
3469 cartesian_waypoints_planner_B.e_h[0] = -cartesian_waypoints_planner_B.e_h[0];
3470 for (m = 2; m < 4; m++) {
3471 cartesian_waypoints_planner_B.work[m - 1] = 0.0;
3472 }
3473
3474 for (m = 2; m < 4; m++) {
3475 cartesian_waypoints_p_xaxpy_evq(2, cartesian_waypoints_planner_B.e_h[m - 1],
3476 cartesian_waypoints_planner_B.A, 3 * (m - 1) + 2,
3477 cartesian_waypoints_planner_B.work, 2);
3478 }
3479
3480 for (m = 2; m < 4; m++) {
3481 memcpy(&cartesian_waypoints_planner_B.A_g[0],
3482 &cartesian_waypoints_planner_B.A[0], 9U * sizeof(real_T));
3483 cartesian_waypoints_pl_xaxpy_ev(2, -cartesian_waypoints_planner_B.e_h[m -
3484 1] / cartesian_waypoints_planner_B.e_h[1],
3485 cartesian_waypoints_planner_B.work, 2, cartesian_waypoints_planner_B.A_g,
3486 (m - 1) * 3 + 2, cartesian_waypoints_planner_B.A);
3487 }
3488 }
3489
3490 for (m = 2; m < 4; m++) {
3491 V[m - 1] = cartesian_waypoints_planner_B.e_h[m - 1];
3492 }
3493
3494 apply_transform = false;
3495 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_plann_xnrm2(2,
3496 cartesian_waypoints_planner_B.A, 5);
3497 if (cartesian_waypoints_planner_B.nrm > 0.0) {
3498 apply_transform = true;
3499 if (cartesian_waypoints_planner_B.A[4] < 0.0) {
3500 cartesian_waypoints_planner_B.s[1] = -cartesian_waypoints_planner_B.nrm;
3501 } else {
3502 cartesian_waypoints_planner_B.s[1] = cartesian_waypoints_planner_B.nrm;
3503 }
3504
3505 if (fabs(cartesian_waypoints_planner_B.s[1]) >= 1.0020841800044864E-292) {
3506 cartesian_waypoints_planner_B.nrm = 1.0 / cartesian_waypoints_planner_B.s
3507 [1];
3508 for (qq = 5; qq < 7; qq++) {
3509 cartesian_waypoints_planner_B.A[qq - 1] *=
3510 cartesian_waypoints_planner_B.nrm;
3511 }
3512 } else {
3513 for (qq = 5; qq < 7; qq++) {
3514 cartesian_waypoints_planner_B.A[qq - 1] /=
3515 cartesian_waypoints_planner_B.s[1];
3516 }
3517 }
3518
3519 cartesian_waypoints_planner_B.A[4]++;
3520 cartesian_waypoints_planner_B.s[1] = -cartesian_waypoints_planner_B.s[1];
3521 } else {
3522 cartesian_waypoints_planner_B.s[1] = 0.0;
3523 }
3524
3525 if (apply_transform) {
3526 for (m = 3; m < 4; m++) {
3527 memcpy(&cartesian_waypoints_planner_B.A_g[0],
3528 &cartesian_waypoints_planner_B.A[0], 9U * sizeof(real_T));
3529 cartesian_waypoints_plann_xaxpy(2, -(cartesian_waypoints_plann_xdotc(2,
3530 cartesian_waypoints_planner_B.A, 5, cartesian_waypoints_planner_B.A, 8) /
3531 cartesian_waypoints_planner_B.A[4]), 5,
3532 cartesian_waypoints_planner_B.A_g, 8, cartesian_waypoints_planner_B.A);
3533 }
3534 }
3535
3536 for (m = 2; m < 4; m++) {
3537 U[m + 2] = cartesian_waypoints_planner_B.A[m + 2];
3538 }
3539
3540 m = 2;
3541 cartesian_waypoints_planner_B.s[2] = cartesian_waypoints_planner_B.A[8];
3542 cartesian_waypoints_planner_B.e_h[1] = cartesian_waypoints_planner_B.A[7];
3543 cartesian_waypoints_planner_B.e_h[2] = 0.0;
3544 U[6] = 0.0;
3545 U[7] = 0.0;
3546 U[8] = 1.0;
3547 for (c_q = 1; c_q >= 0; c_q--) {
3548 qq = 3 * c_q + c_q;
3549 if (cartesian_waypoints_planner_B.s[c_q] != 0.0) {
3550 for (kase = c_q + 2; kase < 4; kase++) {
3551 qjj = ((kase - 1) * 3 + c_q) + 1;
3552 memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));
3553 cartesian_waypoints_plann_xaxpy(3 - c_q,
3554 -(cartesian_waypoints_plann_xdotc(3 - c_q, U, qq + 1, U, qjj) / U[qq]),
3555 qq + 1, cartesian_waypoints_planner_B.A, qjj, U);
3556 }
3557
3558 for (qjj = c_q + 1; qjj < 4; qjj++) {
3559 kase = (3 * c_q + qjj) - 1;
3560 U[kase] = -U[kase];
3561 }
3562
3563 U[qq]++;
3564 if (0 <= c_q - 1) {
3565 U[3 * c_q] = 0.0;
3566 }
3567 } else {
3568 U[3 * c_q] = 0.0;
3569 U[3 * c_q + 1] = 0.0;
3570 U[3 * c_q + 2] = 0.0;
3571 U[qq] = 1.0;
3572 }
3573 }
3574
3575 for (c_q = 2; c_q >= 0; c_q--) {
3576 if ((c_q + 1 <= 1) && (cartesian_waypoints_planner_B.e_h[0] != 0.0)) {
3577 memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3578 cartesian_waypoints_plann_xaxpy(2, -(cartesian_waypoints_plann_xdotc(2, V,
3579 2, V, 5) / V[1]), 2, cartesian_waypoints_planner_B.A, 5, V);
3580 memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3581 cartesian_waypoints_plann_xaxpy(2, -(cartesian_waypoints_plann_xdotc(2, V,
3582 2, V, 8) / V[1]), 2, cartesian_waypoints_planner_B.A, 8, V);
3583 }
3584
3585 V[3 * c_q] = 0.0;
3586 V[3 * c_q + 1] = 0.0;
3587 V[3 * c_q + 2] = 0.0;
3588 V[c_q + 3 * c_q] = 1.0;
3589 }
3590
3591 for (c_q = 0; c_q < 3; c_q++) {
3592 cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.e_h[c_q];
3593 if (cartesian_waypoints_planner_B.s[c_q] != 0.0) {
3594 cartesian_waypoints_planner_B.rt = fabs
3595 (cartesian_waypoints_planner_B.s[c_q]);
3596 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.s[c_q] /
3597 cartesian_waypoints_planner_B.rt;
3598 cartesian_waypoints_planner_B.s[c_q] = cartesian_waypoints_planner_B.rt;
3599 if (c_q + 1 < 3) {
3600 cartesian_waypoints_planner_B.ztest =
3601 cartesian_waypoints_planner_B.e_h[c_q] /
3602 cartesian_waypoints_planner_B.nrm;
3603 }
3604
3605 qjj = 3 * c_q;
3606 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
3607 U[qq - 1] *= cartesian_waypoints_planner_B.nrm;
3608 }
3609 }
3610
3611 if ((c_q + 1 < 3) && (cartesian_waypoints_planner_B.ztest != 0.0)) {
3612 cartesian_waypoints_planner_B.rt = fabs
3613 (cartesian_waypoints_planner_B.ztest);
3614 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt /
3615 cartesian_waypoints_planner_B.ztest;
3616 cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.rt;
3617 cartesian_waypoints_planner_B.s[c_q + 1] *=
3618 cartesian_waypoints_planner_B.nrm;
3619 qjj = (c_q + 1) * 3;
3620 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
3621 V[qq - 1] *= cartesian_waypoints_planner_B.nrm;
3622 }
3623 }
3624
3625 cartesian_waypoints_planner_B.e_h[c_q] = cartesian_waypoints_planner_B.ztest;
3626 }
3627
3628 qq = 0;
3629 cartesian_waypoints_planner_B.nrm = 0.0;
3630 cartesian_waypoints_planner_B.ztest = fabs(cartesian_waypoints_planner_B.s[0]);
3631 cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.e_h[0]);
3632 if ((cartesian_waypoints_planner_B.ztest > cartesian_waypoints_planner_B.rt) ||
3633 rtIsNaN(cartesian_waypoints_planner_B.rt)) {
3634 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;
3635 }
3636
3637 if (!rtIsNaN(cartesian_waypoints_planner_B.rt)) {
3638 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt;
3639 }
3640
3641 cartesian_waypoints_planner_B.ztest = fabs(cartesian_waypoints_planner_B.s[1]);
3642 cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.e_h[1]);
3643 if ((cartesian_waypoints_planner_B.ztest > cartesian_waypoints_planner_B.rt) ||
3644 rtIsNaN(cartesian_waypoints_planner_B.rt)) {
3645 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;
3646 }
3647
3648 if ((!(cartesian_waypoints_planner_B.nrm > cartesian_waypoints_planner_B.rt)) &&
3649 (!rtIsNaN(cartesian_waypoints_planner_B.rt))) {
3650 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt;
3651 }
3652
3653 cartesian_waypoints_planner_B.ztest = fabs(cartesian_waypoints_planner_B.s[2]);
3654 cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.e_h[2]);
3655 if ((cartesian_waypoints_planner_B.ztest > cartesian_waypoints_planner_B.rt) ||
3656 rtIsNaN(cartesian_waypoints_planner_B.rt)) {
3657 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;
3658 }
3659
3660 if ((!(cartesian_waypoints_planner_B.nrm > cartesian_waypoints_planner_B.rt)) &&
3661 (!rtIsNaN(cartesian_waypoints_planner_B.rt))) {
3662 cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt;
3663 }
3664
3665 while ((m + 1 > 0) && (!(qq >= 75))) {
3666 c_q = m;
3667 qjj = m;
3668 exitg1 = false;
3669 while ((!exitg1) && (qjj > -1)) {
3670 c_q = qjj;
3671 if (qjj == 0) {
3672 exitg1 = true;
3673 } else {
3674 cartesian_waypoints_planner_B.rt = fabs
3675 (cartesian_waypoints_planner_B.e_h[qjj - 1]);
3676 if ((cartesian_waypoints_planner_B.rt <= (fabs
3677 (cartesian_waypoints_planner_B.s[qjj - 1]) + fabs
3678 (cartesian_waypoints_planner_B.s[qjj])) * 2.2204460492503131E-16) ||
3679 (cartesian_waypoints_planner_B.rt <= 1.0020841800044864E-292) ||
3680 ((qq > 20) && (cartesian_waypoints_planner_B.rt <=
3681 2.2204460492503131E-16 *
3682 cartesian_waypoints_planner_B.nrm))) {
3683 cartesian_waypoints_planner_B.e_h[qjj - 1] = 0.0;
3684 exitg1 = true;
3685 } else {
3686 qjj--;
3687 }
3688 }
3689 }
3690
3691 if (c_q == m) {
3692 kase = 4;
3693 } else {
3694 qjj = m + 1;
3695 kase = m + 1;
3696 exitg1 = false;
3697 while ((!exitg1) && (kase >= c_q)) {
3698 qjj = kase;
3699 if (kase == c_q) {
3700 exitg1 = true;
3701 } else {
3702 cartesian_waypoints_planner_B.rt = 0.0;
3703 if (kase < m + 1) {
3704 cartesian_waypoints_planner_B.rt = fabs
3705 (cartesian_waypoints_planner_B.e_h[kase - 1]);
3706 }
3707
3708 if (kase > c_q + 1) {
3709 cartesian_waypoints_planner_B.rt += fabs
3710 (cartesian_waypoints_planner_B.e_h[kase - 2]);
3711 }
3712
3713 cartesian_waypoints_planner_B.ztest = fabs
3714 (cartesian_waypoints_planner_B.s[kase - 1]);
3715 if ((cartesian_waypoints_planner_B.ztest <= 2.2204460492503131E-16 *
3716 cartesian_waypoints_planner_B.rt) ||
3717 (cartesian_waypoints_planner_B.ztest <= 1.0020841800044864E-292))
3718 {
3719 cartesian_waypoints_planner_B.s[kase - 1] = 0.0;
3720 exitg1 = true;
3721 } else {
3722 kase--;
3723 }
3724 }
3725 }
3726
3727 if (qjj == c_q) {
3728 kase = 3;
3729 } else if (m + 1 == qjj) {
3730 kase = 1;
3731 } else {
3732 kase = 2;
3733 c_q = qjj;
3734 }
3735 }
3736
3737 switch (kase) {
3738 case 1:
3739 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.e_h[m - 1];
3740 cartesian_waypoints_planner_B.e_h[m - 1] = 0.0;
3741 for (qjj = m; qjj >= c_q + 1; qjj--) {
3742 cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.e_h
3743 [0];
3744 cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.s[qjj - 1],
3745 cartesian_waypoints_planner_B.rt, &cartesian_waypoints_planner_B.s[qjj
3746 - 1], &cartesian_waypoints_planner_B.rt,
3747 &cartesian_waypoints_planner_B.sqds,
3748 &cartesian_waypoints_planner_B.b_l);
3749 if (qjj > c_q + 1) {
3750 cartesian_waypoints_planner_B.rt = -cartesian_waypoints_planner_B.b_l *
3751 cartesian_waypoints_planner_B.e_h[0];
3752 cartesian_waypoints_planner_B.ztest =
3753 cartesian_waypoints_planner_B.e_h[0] *
3754 cartesian_waypoints_planner_B.sqds;
3755 }
3756
3757 memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3758 cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, (qjj -
3759 1) * 3 + 1, 3 * m + 1, cartesian_waypoints_planner_B.sqds,
3760 cartesian_waypoints_planner_B.b_l, V);
3761 cartesian_waypoints_planner_B.e_h[0] =
3762 cartesian_waypoints_planner_B.ztest;
3763 }
3764 break;
3765
3766 case 2:
3767 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.e_h[c_q -
3768 1];
3769 cartesian_waypoints_planner_B.e_h[c_q - 1] = 0.0;
3770 for (qjj = c_q + 1; qjj <= m + 1; qjj++) {
3771 cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.s[qjj - 1],
3772 cartesian_waypoints_planner_B.rt, &cartesian_waypoints_planner_B.s[qjj
3773 - 1], &cartesian_waypoints_planner_B.ztest,
3774 &cartesian_waypoints_planner_B.sqds,
3775 &cartesian_waypoints_planner_B.b_l);
3776 cartesian_waypoints_planner_B.ztest =
3777 cartesian_waypoints_planner_B.e_h[qjj - 1];
3778 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest *
3779 -cartesian_waypoints_planner_B.b_l;
3780 cartesian_waypoints_planner_B.e_h[qjj - 1] =
3781 cartesian_waypoints_planner_B.ztest *
3782 cartesian_waypoints_planner_B.sqds;
3783 memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));
3784 cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, (qjj -
3785 1) * 3 + 1, (c_q - 1) * 3 + 1, cartesian_waypoints_planner_B.sqds,
3786 cartesian_waypoints_planner_B.b_l, U);
3787 }
3788 break;
3789
3790 case 3:
3791 cartesian_waypoints_planner_B.ztest = fabs
3792 (cartesian_waypoints_planner_B.s[m]);
3793 cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.s[m - 1];
3794 cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.sqds);
3795 if ((cartesian_waypoints_planner_B.ztest >
3796 cartesian_waypoints_planner_B.rt) || rtIsNaN
3797 (cartesian_waypoints_planner_B.rt)) {
3798 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;
3799 }
3800
3801 cartesian_waypoints_planner_B.b_l = cartesian_waypoints_planner_B.e_h[m -
3802 1];
3803 cartesian_waypoints_planner_B.ztest = fabs
3804 (cartesian_waypoints_planner_B.b_l);
3805 if ((cartesian_waypoints_planner_B.rt >
3806 cartesian_waypoints_planner_B.ztest) || rtIsNaN
3807 (cartesian_waypoints_planner_B.ztest)) {
3808 cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.rt;
3809 }
3810
3811 cartesian_waypoints_planner_B.rt = fabs
3812 (cartesian_waypoints_planner_B.s[c_q]);
3813 if ((cartesian_waypoints_planner_B.ztest >
3814 cartesian_waypoints_planner_B.rt) || rtIsNaN
3815 (cartesian_waypoints_planner_B.rt)) {
3816 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;
3817 }
3818
3819 cartesian_waypoints_planner_B.ztest = fabs
3820 (cartesian_waypoints_planner_B.e_h[c_q]);
3821 if ((cartesian_waypoints_planner_B.rt >
3822 cartesian_waypoints_planner_B.ztest) || rtIsNaN
3823 (cartesian_waypoints_planner_B.ztest)) {
3824 cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.rt;
3825 }
3826
3827 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.s[m] /
3828 cartesian_waypoints_planner_B.ztest;
3829 cartesian_waypoints_planner_B.smm1 = cartesian_waypoints_planner_B.sqds /
3830 cartesian_waypoints_planner_B.ztest;
3831 cartesian_waypoints_planner_B.emm1 = cartesian_waypoints_planner_B.b_l /
3832 cartesian_waypoints_planner_B.ztest;
3833 cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.s[c_q] /
3834 cartesian_waypoints_planner_B.ztest;
3835 cartesian_waypoints_planner_B.b_l = ((cartesian_waypoints_planner_B.smm1 +
3836 cartesian_waypoints_planner_B.rt) * (cartesian_waypoints_planner_B.smm1
3837 - cartesian_waypoints_planner_B.rt) + cartesian_waypoints_planner_B.emm1
3838 * cartesian_waypoints_planner_B.emm1) / 2.0;
3839 cartesian_waypoints_planner_B.smm1 = cartesian_waypoints_planner_B.rt *
3840 cartesian_waypoints_planner_B.emm1;
3841 cartesian_waypoints_planner_B.smm1 *= cartesian_waypoints_planner_B.smm1;
3842 if ((cartesian_waypoints_planner_B.b_l != 0.0) ||
3843 (cartesian_waypoints_planner_B.smm1 != 0.0)) {
3844 cartesian_waypoints_planner_B.emm1 = sqrt
3845 (cartesian_waypoints_planner_B.b_l * cartesian_waypoints_planner_B.b_l
3846 + cartesian_waypoints_planner_B.smm1);
3847 if (cartesian_waypoints_planner_B.b_l < 0.0) {
3848 cartesian_waypoints_planner_B.emm1 =
3849 -cartesian_waypoints_planner_B.emm1;
3850 }
3851
3852 cartesian_waypoints_planner_B.emm1 = cartesian_waypoints_planner_B.smm1 /
3853 (cartesian_waypoints_planner_B.b_l +
3854 cartesian_waypoints_planner_B.emm1);
3855 } else {
3856 cartesian_waypoints_planner_B.emm1 = 0.0;
3857 }
3858
3859 cartesian_waypoints_planner_B.rt = (cartesian_waypoints_planner_B.sqds +
3860 cartesian_waypoints_planner_B.rt) * (cartesian_waypoints_planner_B.sqds
3861 - cartesian_waypoints_planner_B.rt) + cartesian_waypoints_planner_B.emm1;
3862 cartesian_waypoints_planner_B.sqds *=
3863 cartesian_waypoints_planner_B.e_h[c_q] /
3864 cartesian_waypoints_planner_B.ztest;
3865 for (d_k = c_q + 1; d_k <= m; d_k++) {
3866 cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.rt,
3867 cartesian_waypoints_planner_B.sqds,
3868 &cartesian_waypoints_planner_B.ztest,
3869 &cartesian_waypoints_planner_B.emm1,
3870 &cartesian_waypoints_planner_B.b_l,
3871 &cartesian_waypoints_planner_B.smm1);
3872 if (d_k > c_q + 1) {
3873 cartesian_waypoints_planner_B.e_h[0] =
3874 cartesian_waypoints_planner_B.ztest;
3875 }
3876
3877 cartesian_waypoints_planner_B.ztest =
3878 cartesian_waypoints_planner_B.e_h[d_k - 1];
3879 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.s[d_k -
3880 1];
3881 cartesian_waypoints_planner_B.e_h[d_k - 1] =
3882 cartesian_waypoints_planner_B.ztest *
3883 cartesian_waypoints_planner_B.b_l - cartesian_waypoints_planner_B.rt *
3884 cartesian_waypoints_planner_B.smm1;
3885 cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.smm1 *
3886 cartesian_waypoints_planner_B.s[d_k];
3887 cartesian_waypoints_planner_B.s[d_k] *=
3888 cartesian_waypoints_planner_B.b_l;
3889 qjj = (d_k - 1) * 3 + 1;
3890 kase = 3 * d_k + 1;
3891 memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3892 cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, qjj,
3893 kase, cartesian_waypoints_planner_B.b_l,
3894 cartesian_waypoints_planner_B.smm1, V);
3895 cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.rt *
3896 cartesian_waypoints_planner_B.b_l +
3897 cartesian_waypoints_planner_B.ztest *
3898 cartesian_waypoints_planner_B.smm1, cartesian_waypoints_planner_B.sqds,
3899 &cartesian_waypoints_planner_B.s[d_k - 1],
3900 &cartesian_waypoints_planner_B.unusedU2,
3901 &cartesian_waypoints_planner_B.emm1,
3902 &cartesian_waypoints_planner_B.d_sn);
3903 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.e_h[d_k
3904 - 1] * cartesian_waypoints_planner_B.emm1 +
3905 cartesian_waypoints_planner_B.d_sn *
3906 cartesian_waypoints_planner_B.s[d_k];
3907 cartesian_waypoints_planner_B.s[d_k] =
3908 cartesian_waypoints_planner_B.e_h[d_k - 1] *
3909 -cartesian_waypoints_planner_B.d_sn +
3910 cartesian_waypoints_planner_B.emm1 *
3911 cartesian_waypoints_planner_B.s[d_k];
3912 cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.d_sn *
3913 cartesian_waypoints_planner_B.e_h[d_k];
3914 cartesian_waypoints_planner_B.e_h[d_k] *=
3915 cartesian_waypoints_planner_B.emm1;
3916 memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));
3917 cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, qjj,
3918 kase, cartesian_waypoints_planner_B.emm1,
3919 cartesian_waypoints_planner_B.d_sn, U);
3920 }
3921
3922 cartesian_waypoints_planner_B.e_h[m - 1] =
3923 cartesian_waypoints_planner_B.rt;
3924 qq++;
3925 break;
3926
3927 default:
3928 if (cartesian_waypoints_planner_B.s[c_q] < 0.0) {
3929 cartesian_waypoints_planner_B.s[c_q] =
3930 -cartesian_waypoints_planner_B.s[c_q];
3931 qjj = 3 * c_q;
3932 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
3933 V[qq - 1] = -V[qq - 1];
3934 }
3935 }
3936
3937 qq = c_q + 1;
3938 while ((c_q + 1 < 3) && (cartesian_waypoints_planner_B.s[c_q] <
3939 cartesian_waypoints_planner_B.s[qq])) {
3940 cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.s[c_q];
3941 cartesian_waypoints_planner_B.s[c_q] =
3942 cartesian_waypoints_planner_B.s[qq];
3943 cartesian_waypoints_planner_B.s[qq] = cartesian_waypoints_planner_B.rt;
3944 qjj = 3 * c_q + 1;
3945 kase = (c_q + 1) * 3 + 1;
3946 memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));
3947 cartesian_waypoints_plann_xswap(cartesian_waypoints_planner_B.A, qjj,
3948 kase, V);
3949 memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));
3950 cartesian_waypoints_plann_xswap(cartesian_waypoints_planner_B.A, qjj,
3951 kase, U);
3952 c_q = qq;
3953 qq++;
3954 }
3955
3956 qq = 0;
3957 m--;
3958 break;
3959 }
3960 }
3961
3962 s[0] = cartesian_waypoints_planner_B.s[0];
3963 s[1] = cartesian_waypoints_planner_B.s[1];
3964 s[2] = cartesian_waypoints_planner_B.s[2];
3965}
3966
3967static void cartesian_waypoints__rotm2axang(const real_T R[9], real_T axang[4])
3968{
3969 boolean_T e;
3970 boolean_T p;
3971 boolean_T rEQ0;
3972 int32_T loop_ub_tmp;
3973 boolean_T exitg1;
3974 cartesian_waypoints_planner_B.u.re = (((R[0] + R[4]) + R[8]) - 1.0) * 0.5;
3975 if (!(fabs(cartesian_waypoints_planner_B.u.re) > 1.0)) {
3976 cartesian_waypoints_planner_B.v_a.re = acos
3977 (cartesian_waypoints_planner_B.u.re);
3978 } else {
3979 cartesian_waypoints_planner_B.u_k.re = cartesian_waypoints_planner_B.u.re +
3980 1.0;
3981 cartesian_waypoints_planner_B.u_k.im = 0.0;
3982 cartesian_waypoints_planner_B.dc.re = 1.0 -
3983 cartesian_waypoints_planner_B.u.re;
3984 cartesian_waypoints_planner_B.dc.im = 0.0;
3985 cartesian_waypoints_planner_B.v_a.re = 2.0 * rt_atan2d_snf
3986 ((cartesian_waypoints_planne_sqrt(cartesian_waypoints_planner_B.dc)).re,
3987 (cartesian_waypoints_planne_sqrt(cartesian_waypoints_planner_B.u_k)).re);
3988 }
3989
3990 cartesian_waypoints_planner_B.a_gs = 2.0 * sin
3991 (cartesian_waypoints_planner_B.v_a.re);
3992 cartesian_waypoints_planner_B.v_fb[0] = (R[5] - R[7]) /
3993 cartesian_waypoints_planner_B.a_gs;
3994 cartesian_waypoints_planner_B.v_fb[1] = (R[6] - R[2]) /
3995 cartesian_waypoints_planner_B.a_gs;
3996 cartesian_waypoints_planner_B.v_fb[2] = (R[1] - R[3]) /
3997 cartesian_waypoints_planner_B.a_gs;
3998 if (rtIsNaN(cartesian_waypoints_planner_B.v_a.re) || rtIsInf
3999 (cartesian_waypoints_planner_B.v_a.re)) {
4000 cartesian_waypoints_planner_B.a_gs = (rtNaN);
4001 } else if (cartesian_waypoints_planner_B.v_a.re == 0.0) {
4002 cartesian_waypoints_planner_B.a_gs = 0.0;
4003 } else {
4004 cartesian_waypoints_planner_B.a_gs = fmod
4005 (cartesian_waypoints_planner_B.v_a.re, 3.1415926535897931);
4006 rEQ0 = (cartesian_waypoints_planner_B.a_gs == 0.0);
4007 if (!rEQ0) {
4008 cartesian_waypoints_planner_B.q_n = fabs
4009 (cartesian_waypoints_planner_B.v_a.re / 3.1415926535897931);
4010 rEQ0 = !(fabs(cartesian_waypoints_planner_B.q_n - floor
4011 (cartesian_waypoints_planner_B.q_n + 0.5)) >
4012 2.2204460492503131E-16 * cartesian_waypoints_planner_B.q_n);
4013 }
4014
4015 if (rEQ0) {
4016 cartesian_waypoints_planner_B.a_gs = 0.0;
4017 } else {
4018 if (cartesian_waypoints_planner_B.v_a.re < 0.0) {
4019 cartesian_waypoints_planner_B.a_gs += 3.1415926535897931;
4020 }
4021 }
4022 }
4023
4024 rEQ0 = (cartesian_waypoints_planner_B.a_gs == 0.0);
4025 e = true;
4026 cartesian_waypoints_planner_B.b_k_e = 0;
4027 exitg1 = false;
4028 while ((!exitg1) && (cartesian_waypoints_planner_B.b_k_e < 3)) {
4029 if (!(cartesian_waypoints_planner_B.v_fb[cartesian_waypoints_planner_B.b_k_e]
4030 == 0.0)) {
4031 e = false;
4032 exitg1 = true;
4033 } else {
4034 cartesian_waypoints_planner_B.b_k_e++;
4035 }
4036 }
4037
4038 if (rEQ0 || e) {
4039 loop_ub_tmp = (rEQ0 || e);
4040 cartesian_waypoints_planner_B.loop_ub_j = loop_ub_tmp * 3 - 1;
4041 if (0 <= cartesian_waypoints_planner_B.loop_ub_j) {
4042 memset(&cartesian_waypoints_planner_B.vspecial_data[0], 0,
4043 (cartesian_waypoints_planner_B.loop_ub_j + 1) * sizeof(real_T));
4044 }
4045
4046 loop_ub_tmp--;
4047 for (cartesian_waypoints_planner_B.loop_ub_j = 0;
4048 cartesian_waypoints_planner_B.loop_ub_j <= loop_ub_tmp;
4049 cartesian_waypoints_planner_B.loop_ub_j++) {
4050 memset(&cartesian_waypoints_planner_B.b_I[0], 0, 9U * sizeof(real_T));
4051 cartesian_waypoints_planner_B.b_I[0] = 1.0;
4052 cartesian_waypoints_planner_B.b_I[4] = 1.0;
4053 cartesian_waypoints_planner_B.b_I[8] = 1.0;
4054 p = true;
4055 for (cartesian_waypoints_planner_B.b_k_e = 0;
4056 cartesian_waypoints_planner_B.b_k_e < 9;
4057 cartesian_waypoints_planner_B.b_k_e++) {
4058 cartesian_waypoints_planner_B.a_gs =
4059 cartesian_waypoints_planner_B.b_I[cartesian_waypoints_planner_B.b_k_e]
4060 - R[cartesian_waypoints_planner_B.b_k_e];
4061 if (p && ((!rtIsInf(cartesian_waypoints_planner_B.a_gs)) && (!rtIsNaN
4062 (cartesian_waypoints_planner_B.a_gs)))) {
4063 } else {
4064 p = false;
4065 }
4066
4067 cartesian_waypoints_planner_B.b_I[cartesian_waypoints_planner_B.b_k_e] =
4068 cartesian_waypoints_planner_B.a_gs;
4069 }
4070
4071 if (p) {
4072 cartesian_waypoints_planner_svd(cartesian_waypoints_planner_B.b_I,
4073 cartesian_waypoints_planner_B.b_U,
4074 cartesian_waypoints_planner_B.vspecial_data,
4075 cartesian_waypoints_planner_B.V_f);
4076 } else {
4077 for (cartesian_waypoints_planner_B.b_k_e = 0;
4078 cartesian_waypoints_planner_B.b_k_e < 9;
4079 cartesian_waypoints_planner_B.b_k_e++) {
4080 cartesian_waypoints_planner_B.V_f[cartesian_waypoints_planner_B.b_k_e]
4081 = (rtNaN);
4082 }
4083 }
4084
4085 cartesian_waypoints_planner_B.vspecial_data[0] =
4086 cartesian_waypoints_planner_B.V_f[6];
4087 cartesian_waypoints_planner_B.vspecial_data[1] =
4088 cartesian_waypoints_planner_B.V_f[7];
4089 cartesian_waypoints_planner_B.vspecial_data[2] =
4090 cartesian_waypoints_planner_B.V_f[8];
4091 }
4092
4093 loop_ub_tmp = 0;
4094 if (rEQ0 || e) {
4095 for (cartesian_waypoints_planner_B.loop_ub_j = 0;
4096 cartesian_waypoints_planner_B.loop_ub_j < 1;
4097 cartesian_waypoints_planner_B.loop_ub_j++) {
4098 loop_ub_tmp++;
4099 }
4100 }
4101
4102 for (cartesian_waypoints_planner_B.b_k_e = 0;
4103 cartesian_waypoints_planner_B.b_k_e < loop_ub_tmp;
4104 cartesian_waypoints_planner_B.b_k_e++) {
4105 cartesian_waypoints_planner_B.v_fb[0] =
4106 cartesian_waypoints_planner_B.vspecial_data[3 *
4107 cartesian_waypoints_planner_B.b_k_e];
4108 cartesian_waypoints_planner_B.v_fb[1] =
4109 cartesian_waypoints_planner_B.vspecial_data[3 *
4110 cartesian_waypoints_planner_B.b_k_e + 1];
4111 cartesian_waypoints_planner_B.v_fb[2] =
4112 cartesian_waypoints_planner_B.vspecial_data[3 *
4113 cartesian_waypoints_planner_B.b_k_e + 2];
4114 }
4115 }
4116
4117 cartesian_waypoints_planner_B.a_gs = 1.0 / sqrt
4118 ((cartesian_waypoints_planner_B.v_fb[0] *
4119 cartesian_waypoints_planner_B.v_fb[0] +
4120 cartesian_waypoints_planner_B.v_fb[1] *
4121 cartesian_waypoints_planner_B.v_fb[1]) +
4122 cartesian_waypoints_planner_B.v_fb[2] * cartesian_waypoints_planner_B.v_fb
4123 [2]);
4124 cartesian_waypoints_planner_B.v_fb[0] *= cartesian_waypoints_planner_B.a_gs;
4125 cartesian_waypoints_planner_B.v_fb[1] *= cartesian_waypoints_planner_B.a_gs;
4126 axang[0] = cartesian_waypoints_planner_B.v_fb[0];
4127 axang[1] = cartesian_waypoints_planner_B.v_fb[1];
4128 axang[2] = cartesian_waypoints_planner_B.v_fb[2] *
4129 cartesian_waypoints_planner_B.a_gs;
4130 axang[3] = cartesian_waypoints_planner_B.v_a.re;
4131}
4132
4133static void cartesian_IKHelpers_computeCost(const real_T x[6],
4134 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
4135 emxArray_real_T_cartesian_way_T *Jac, f_robotics_manip_internal_IKE_T **b_args)
4136{
4137 x_robotics_manip_internal_Rig_T *treeInternal;
4138 emxArray_char_T_cartesian_way_T *bodyName;
4139 emxArray_real_T_cartesian_way_T *J;
4140 emxArray_real_T_cartesian_way_T *y;
4141 cartesian_waypoi_emxInit_char_T(&bodyName, 2);
4142 *b_args = args;
4143 treeInternal = args->Robot;
4144 cartesian_waypoints_planner_B.b_j_j = bodyName->size[0] * bodyName->size[1];
4145 bodyName->size[0] = 1;
4146 bodyName->size[1] = args->BodyName->size[1];
4147 cartes_emxEnsureCapacity_char_T(bodyName, cartesian_waypoints_planner_B.b_j_j);
4148 cartesian_waypoints_planner_B.loop_ub_o = args->BodyName->size[0] *
4149 args->BodyName->size[1] - 1;
4150 for (cartesian_waypoints_planner_B.b_j_j = 0;
4151 cartesian_waypoints_planner_B.b_j_j <=
4152 cartesian_waypoints_planner_B.loop_ub_o;
4153 cartesian_waypoints_planner_B.b_j_j++) {
4154 bodyName->data[cartesian_waypoints_planner_B.b_j_j] = args->BodyName->
4155 data[cartesian_waypoints_planner_B.b_j_j];
4156 }
4157
4158 for (cartesian_waypoints_planner_B.b_j_j = 0;
4159 cartesian_waypoints_planner_B.b_j_j < 16;
4160 cartesian_waypoints_planner_B.b_j_j++) {
4161 cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.b_j_j] =
4162 args->Tform[cartesian_waypoints_planner_B.b_j_j];
4163 }
4164
4165 for (cartesian_waypoints_planner_B.b_j_j = 0;
4166 cartesian_waypoints_planner_B.b_j_j < 36;
4167 cartesian_waypoints_planner_B.b_j_j++) {
4168 W[cartesian_waypoints_planner_B.b_j_j] = args->
4169 WeightMatrix[cartesian_waypoints_planner_B.b_j_j];
4170 }
4171
4172 cartesian_waypoi_emxInit_real_T(&J, 2);
4173 RigidBodyTree_efficientFKAndJac(treeInternal, x, bodyName,
4174 cartesian_waypoints_planner_B.T_data, cartesian_waypoints_planner_B.T_size,
4175 J);
4176 cartesian_waypoints_planner_B.b_j_j = Jac->size[0] * Jac->size[1];
4177 Jac->size[0] = 6;
4178 Jac->size[1] = J->size[1];
4179 cartes_emxEnsureCapacity_real_T(Jac, cartesian_waypoints_planner_B.b_j_j);
4180 cartesian_waypoints_planner_B.loop_ub_o = J->size[0] * J->size[1] - 1;
4181 cartesian_waypoi_emxFree_char_T(&bodyName);
4182 for (cartesian_waypoints_planner_B.b_j_j = 0;
4183 cartesian_waypoints_planner_B.b_j_j <=
4184 cartesian_waypoints_planner_B.loop_ub_o;
4185 cartesian_waypoints_planner_B.b_j_j++) {
4186 Jac->data[cartesian_waypoints_planner_B.b_j_j] = -J->
4187 data[cartesian_waypoints_planner_B.b_j_j];
4188 }
4189
4190 cartesian_waypoi_emxFree_real_T(&J);
4191 for (cartesian_waypoints_planner_B.b_j_j = 0;
4192 cartesian_waypoints_planner_B.b_j_j < 3;
4193 cartesian_waypoints_planner_B.b_j_j++) {
4194 cartesian_waypoints_planner_B.T[3 * cartesian_waypoints_planner_B.b_j_j] =
4195 cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.b_j_j];
4196 cartesian_waypoints_planner_B.n_h = 3 * cartesian_waypoints_planner_B.b_j_j
4197 + 1;
4198 cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.n_h] =
4199 cartesian_waypoints_planner_B.T_data[((cartesian_waypoints_planner_B.b_j_j
4200 + 1) + cartesian_waypoints_planner_B.T_size[0]) - 1];
4201 cartesian_waypoints_planner_B.boffset_k = 3 *
4202 cartesian_waypoints_planner_B.b_j_j + 2;
4203 cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.boffset_k] =
4204 cartesian_waypoints_planner_B.T_data[((cartesian_waypoints_planner_B.b_j_j
4205 + 1) + (cartesian_waypoints_planner_B.T_size[0] << 1)) - 1];
4206 for (cartesian_waypoints_planner_B.loop_ub_o = 0;
4207 cartesian_waypoints_planner_B.loop_ub_o < 3;
4208 cartesian_waypoints_planner_B.loop_ub_o++) {
4209 cartesian_waypoints_planner_B.Td_tmp =
4210 cartesian_waypoints_planner_B.loop_ub_o + 3 *
4211 cartesian_waypoints_planner_B.b_j_j;
4212 cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] =
4213 0.0;
4214 cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] +=
4215 cartesian_waypoints_planner_B.T[3 * cartesian_waypoints_planner_B.b_j_j]
4216 * cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.loop_ub_o];
4217 cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] +=
4218 cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.n_h] *
4219 cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.loop_ub_o
4220 + 4];
4221 cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] +=
4222 cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.boffset_k]
4223 * cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.loop_ub_o
4224 + 8];
4225 }
4226 }
4227
4228 cartesian_waypoints__rotm2axang(cartesian_waypoints_planner_B.Td_a,
4229 cartesian_waypoints_planner_B.v);
4230 cartesian_waypoints_planner_B.e[0] = cartesian_waypoints_planner_B.v[3] *
4231 cartesian_waypoints_planner_B.v[0];
4232 cartesian_waypoints_planner_B.e[3] = cartesian_waypoints_planner_B.Td[12] -
4233 cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.T_size[0]
4234 * 3];
4235 cartesian_waypoints_planner_B.e[1] = cartesian_waypoints_planner_B.v[3] *
4236 cartesian_waypoints_planner_B.v[1];
4237 cartesian_waypoints_planner_B.e[4] = cartesian_waypoints_planner_B.Td[13] -
4238 cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.T_size[0]
4239 * 3 + 1];
4240 cartesian_waypoints_planner_B.e[2] = cartesian_waypoints_planner_B.v[3] *
4241 cartesian_waypoints_planner_B.v[2];
4242 cartesian_waypoints_planner_B.e[5] = cartesian_waypoints_planner_B.Td[14] -
4243 cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.T_size[0]
4244 * 3 + 2];
4245 cartesian_waypoints_planner_B.b_j_j = args->ErrTemp->size[0];
4246 args->ErrTemp->size[0] = 6;
4247 cartes_emxEnsureCapacity_real_T(args->ErrTemp,
4248 cartesian_waypoints_planner_B.b_j_j);
4249 for (cartesian_waypoints_planner_B.b_j_j = 0;
4250 cartesian_waypoints_planner_B.b_j_j < 6;
4251 cartesian_waypoints_planner_B.b_j_j++) {
4252 args->ErrTemp->data[cartesian_waypoints_planner_B.b_j_j] =
4253 cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.b_j_j];
4254 }
4255
4256 for (cartesian_waypoints_planner_B.b_j_j = 0;
4257 cartesian_waypoints_planner_B.b_j_j < 6;
4258 cartesian_waypoints_planner_B.b_j_j++) {
4259 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_j] = 0.0;
4260 for (cartesian_waypoints_planner_B.loop_ub_o = 0;
4261 cartesian_waypoints_planner_B.loop_ub_o < 6;
4262 cartesian_waypoints_planner_B.loop_ub_o++) {
4263 cartesian_waypoints_planner_B.s_c = W[6 *
4264 cartesian_waypoints_planner_B.b_j_j +
4265 cartesian_waypoints_planner_B.loop_ub_o] * (0.5 *
4266 cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.loop_ub_o])
4267 + cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_j];
4268 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_j] =
4269 cartesian_waypoints_planner_B.s_c;
4270 }
4271 }
4272
4273 cartesian_waypoints_planner_B.s_c = 0.0;
4274 for (cartesian_waypoints_planner_B.b_j_j = 0;
4275 cartesian_waypoints_planner_B.b_j_j < 6;
4276 cartesian_waypoints_planner_B.b_j_j++) {
4277 cartesian_waypoints_planner_B.s_c +=
4278 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_j] *
4279 cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.b_j_j];
4280 }
4281
4282 args->CostTemp = cartesian_waypoints_planner_B.s_c;
4283 for (cartesian_waypoints_planner_B.b_j_j = 0;
4284 cartesian_waypoints_planner_B.b_j_j < 6;
4285 cartesian_waypoints_planner_B.b_j_j++) {
4286 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_j] = 0.0;
4287 for (cartesian_waypoints_planner_B.loop_ub_o = 0;
4288 cartesian_waypoints_planner_B.loop_ub_o < 6;
4289 cartesian_waypoints_planner_B.loop_ub_o++) {
4290 cartesian_waypoints_planner_B.s_c = W[6 *
4291 cartesian_waypoints_planner_B.b_j_j +
4292 cartesian_waypoints_planner_B.loop_ub_o] *
4293 cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.loop_ub_o]
4294 + cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_j];
4295 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_j] =
4296 cartesian_waypoints_planner_B.s_c;
4297 }
4298 }
4299
4300 cartesian_waypoi_emxInit_real_T(&y, 2);
4301 cartesian_waypoints_planner_B.n_h = Jac->size[1] - 1;
4302 cartesian_waypoints_planner_B.b_j_j = y->size[0] * y->size[1];
4303 y->size[0] = 1;
4304 y->size[1] = Jac->size[1];
4305 cartes_emxEnsureCapacity_real_T(y, cartesian_waypoints_planner_B.b_j_j);
4306 for (cartesian_waypoints_planner_B.b_j_j = 0;
4307 cartesian_waypoints_planner_B.b_j_j <= cartesian_waypoints_planner_B.n_h;
4308 cartesian_waypoints_planner_B.b_j_j++) {
4309 cartesian_waypoints_planner_B.boffset_k =
4310 cartesian_waypoints_planner_B.b_j_j * 6 - 1;
4311 cartesian_waypoints_planner_B.s_c = 0.0;
4312 for (cartesian_waypoints_planner_B.loop_ub_o = 0;
4313 cartesian_waypoints_planner_B.loop_ub_o < 6;
4314 cartesian_waypoints_planner_B.loop_ub_o++) {
4315 cartesian_waypoints_planner_B.s_c += Jac->data
4316 [(cartesian_waypoints_planner_B.boffset_k +
4317 cartesian_waypoints_planner_B.loop_ub_o) + 1] *
4318 cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.loop_ub_o];
4319 }
4320
4321 y->data[cartesian_waypoints_planner_B.b_j_j] =
4322 cartesian_waypoints_planner_B.s_c;
4323 }
4324
4325 cartesian_waypoints_planner_B.b_j_j = args->GradTemp->size[0];
4326 args->GradTemp->size[0] = y->size[1];
4327 cartes_emxEnsureCapacity_real_T(args->GradTemp,
4328 cartesian_waypoints_planner_B.b_j_j);
4329 cartesian_waypoints_planner_B.loop_ub_o = y->size[1];
4330 for (cartesian_waypoints_planner_B.b_j_j = 0;
4331 cartesian_waypoints_planner_B.b_j_j <
4332 cartesian_waypoints_planner_B.loop_ub_o;
4333 cartesian_waypoints_planner_B.b_j_j++) {
4334 args->GradTemp->data[cartesian_waypoints_planner_B.b_j_j] = y->
4335 data[cartesian_waypoints_planner_B.b_j_j];
4336 }
4337
4338 cartesian_waypoi_emxFree_real_T(&y);
4339 cartesian_waypoints_planner_B.s_c = args->CostTemp;
4340 *cost = cartesian_waypoints_planner_B.s_c;
4341}
4342
4343static void cartesian_waypoints_planner_eye(real_T b_I[36])
4344{
4345 int32_T b_k;
4346 memset(&b_I[0], 0, 36U * sizeof(real_T));
4347 for (b_k = 0; b_k < 6; b_k++) {
4348 b_I[b_k + 6 * b_k] = 1.0;
4349 }
4350}
4351
4352static void cartesian_way_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
4353 **pEmxArray, int32_T numDimensions)
4354{
4355 emxArray_boolean_T_cartesian__T *emxArray;
4356 int32_T i;
4357 *pEmxArray = (emxArray_boolean_T_cartesian__T *)malloc(sizeof
4358 (emxArray_boolean_T_cartesian__T));
4359 emxArray = *pEmxArray;
4360 emxArray->data = (boolean_T *)NULL;
4361 emxArray->numDimensions = numDimensions;
4362 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
4363 emxArray->allocatedSize = 0;
4364 emxArray->canFreeData = true;
4365 for (i = 0; i < numDimensions; i++) {
4366 emxArray->size[i] = 0;
4367 }
4368}
4369
4370static void cartesian_waypo_emxInit_int32_T(emxArray_int32_T_cartesian_wa_T
4371 **pEmxArray, int32_T numDimensions)
4372{
4373 emxArray_int32_T_cartesian_wa_T *emxArray;
4374 int32_T i;
4375 *pEmxArray = (emxArray_int32_T_cartesian_wa_T *)malloc(sizeof
4376 (emxArray_int32_T_cartesian_wa_T));
4377 emxArray = *pEmxArray;
4378 emxArray->data = (int32_T *)NULL;
4379 emxArray->numDimensions = numDimensions;
4380 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
4381 emxArray->allocatedSize = 0;
4382 emxArray->canFreeData = true;
4383 for (i = 0; i < numDimensions; i++) {
4384 emxArray->size[i] = 0;
4385 }
4386}
4387
4388static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
4389 *emxArray, int32_T oldNumel)
4390{
4391 int32_T newNumel;
4392 int32_T i;
4393 void *newData;
4394 if (oldNumel < 0) {
4395 oldNumel = 0;
4396 }
4397
4398 newNumel = 1;
4399 for (i = 0; i < emxArray->numDimensions; i++) {
4400 newNumel *= emxArray->size[i];
4401 }
4402
4403 if (newNumel > emxArray->allocatedSize) {
4404 i = emxArray->allocatedSize;
4405 if (i < 16) {
4406 i = 16;
4407 }
4408
4409 while (i < newNumel) {
4410 if (i > 1073741823) {
4411 i = MAX_int32_T;
4412 } else {
4413 i <<= 1;
4414 }
4415 }
4416
4417 newData = calloc(static_cast<uint32_T>(i), sizeof(boolean_T));
4418 if (emxArray->data != NULL) {
4419 memcpy(newData, emxArray->data, sizeof(boolean_T) * oldNumel);
4420 if (emxArray->canFreeData) {
4421 free(emxArray->data);
4422 }
4423 }
4424
4425 emxArray->data = (boolean_T *)newData;
4426 emxArray->allocatedSize = i;
4427 emxArray->canFreeData = true;
4428 }
4429}
4430
4431static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_wa_T
4432 *emxArray, int32_T oldNumel)
4433{
4434 int32_T newNumel;
4435 int32_T i;
4436 void *newData;
4437 if (oldNumel < 0) {
4438 oldNumel = 0;
4439 }
4440
4441 newNumel = 1;
4442 for (i = 0; i < emxArray->numDimensions; i++) {
4443 newNumel *= emxArray->size[i];
4444 }
4445
4446 if (newNumel > emxArray->allocatedSize) {
4447 i = emxArray->allocatedSize;
4448 if (i < 16) {
4449 i = 16;
4450 }
4451
4452 while (i < newNumel) {
4453 if (i > 1073741823) {
4454 i = MAX_int32_T;
4455 } else {
4456 i <<= 1;
4457 }
4458 }
4459
4460 newData = calloc(static_cast<uint32_T>(i), sizeof(int32_T));
4461 if (emxArray->data != NULL) {
4462 memcpy(newData, emxArray->data, sizeof(int32_T) * oldNumel);
4463 if (emxArray->canFreeData) {
4464 free(emxArray->data);
4465 }
4466 }
4467
4468 emxArray->data = (int32_T *)newData;
4469 emxArray->allocatedSize = i;
4470 emxArray->canFreeData = true;
4471 }
4472}
4473
4474static real_T cartesian_waypoints_plan_norm_e(const real_T x[6])
4475{
4476 real_T y;
4477 real_T scale;
4478 real_T absxk;
4479 real_T t;
4480 int32_T b_k;
4481 y = 0.0;
4482 scale = 3.3121686421112381E-170;
4483 for (b_k = 0; b_k < 6; b_k++) {
4484 absxk = fabs(x[b_k]);
4485 if (absxk > scale) {
4486 t = scale / absxk;
4487 y = y * t * t + 1.0;
4488 scale = absxk;
4489 } else {
4490 t = absxk / scale;
4491 y += t * t;
4492 }
4493 }
4494
4495 return scale * sqrt(y);
4496}
4497
4498static real_T SystemTimeProvider_getElapsedTi(const
4499 f_robotics_core_internal_Syst_T *obj)
4500{
4501 real_T systemTime;
4502 systemTime = ctimefun();
4503 return systemTime - obj->StartTime;
4504}
4505
4506static real_T cartesian_waypoints_pl_xnrm2_ev(int32_T n, const
4507 emxArray_real_T_cartesian_way_T *x, int32_T ix0)
4508{
4509 real_T y;
4510 y = 0.0;
4511 if (n >= 1) {
4512 if (n == 1) {
4513 y = fabs(x->data[ix0 - 1]);
4514 } else {
4515 cartesian_waypoints_planner_B.scale_i3 = 3.3121686421112381E-170;
4516 cartesian_waypoints_planner_B.kend_d = ix0 + n;
4517 for (cartesian_waypoints_planner_B.k_j1 = ix0;
4518 cartesian_waypoints_planner_B.k_j1 <
4519 cartesian_waypoints_planner_B.kend_d;
4520 cartesian_waypoints_planner_B.k_j1++) {
4521 cartesian_waypoints_planner_B.absxk_k = fabs(x->
4522 data[cartesian_waypoints_planner_B.k_j1 - 1]);
4523 if (cartesian_waypoints_planner_B.absxk_k >
4524 cartesian_waypoints_planner_B.scale_i3) {
4525 cartesian_waypoints_planner_B.t_f =
4526 cartesian_waypoints_planner_B.scale_i3 /
4527 cartesian_waypoints_planner_B.absxk_k;
4528 y = y * cartesian_waypoints_planner_B.t_f *
4529 cartesian_waypoints_planner_B.t_f + 1.0;
4530 cartesian_waypoints_planner_B.scale_i3 =
4531 cartesian_waypoints_planner_B.absxk_k;
4532 } else {
4533 cartesian_waypoints_planner_B.t_f =
4534 cartesian_waypoints_planner_B.absxk_k /
4535 cartesian_waypoints_planner_B.scale_i3;
4536 y += cartesian_waypoints_planner_B.t_f *
4537 cartesian_waypoints_planner_B.t_f;
4538 }
4539 }
4540
4541 y = cartesian_waypoints_planner_B.scale_i3 * sqrt(y);
4542 }
4543 }
4544
4545 return y;
4546}
4547
4548static void cartesian_waypoints_plan_qrpf_e(const
4549 emxArray_real_T_cartesian_way_T *A, int32_T m, int32_T n,
4550 emxArray_real_T_cartesian_way_T *tau, const emxArray_int32_T_cartesian_wa_T
4551 *jpvt, emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T
4552 *b_jpvt)
4553{
4554 emxArray_real_T_cartesian_way_T *work;
4555 emxArray_real_T_cartesian_way_T *vn1;
4556 emxArray_real_T_cartesian_way_T *vn2;
4557 emxArray_real_T_cartesian_way_T *c_x;
4558 int32_T exitg1;
4559 boolean_T exitg2;
4560 cartesian_waypoints_planner_B.kend = b_jpvt->size[0] * b_jpvt->size[1];
4561 b_jpvt->size[0] = 1;
4562 b_jpvt->size[1] = jpvt->size[1];
4563 carte_emxEnsureCapacity_int32_T(b_jpvt, cartesian_waypoints_planner_B.kend);
4564 cartesian_waypoints_planner_B.ix_f = jpvt->size[0] * jpvt->size[1] - 1;
4565 for (cartesian_waypoints_planner_B.kend = 0;
4566 cartesian_waypoints_planner_B.kend <= cartesian_waypoints_planner_B.ix_f;
4567 cartesian_waypoints_planner_B.kend++) {
4568 b_jpvt->data[cartesian_waypoints_planner_B.kend] = jpvt->
4569 data[cartesian_waypoints_planner_B.kend];
4570 }
4571
4572 cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];
4573 b_A->size[0] = A->size[0];
4574 b_A->size[1] = A->size[1];
4575 cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);
4576 cartesian_waypoints_planner_B.ix_f = A->size[0] * A->size[1] - 1;
4577 for (cartesian_waypoints_planner_B.kend = 0;
4578 cartesian_waypoints_planner_B.kend <= cartesian_waypoints_planner_B.ix_f;
4579 cartesian_waypoints_planner_B.kend++) {
4580 b_A->data[cartesian_waypoints_planner_B.kend] = A->
4581 data[cartesian_waypoints_planner_B.kend];
4582 }
4583
4584 cartesian_waypoi_emxInit_real_T(&work, 1);
4585 cartesian_waypoints_planner_B.ma = A->size[0];
4586 if (m < n) {
4587 cartesian_waypoints_planner_B.m_hj = m;
4588 } else {
4589 cartesian_waypoints_planner_B.m_hj = n;
4590 }
4591
4592 cartesian_waypoints_planner_B.minmn_a = cartesian_waypoints_planner_B.m_hj - 1;
4593 cartesian_waypoints_planner_B.kend = work->size[0];
4594 work->size[0] = A->size[1];
4595 cartes_emxEnsureCapacity_real_T(work, cartesian_waypoints_planner_B.kend);
4596 cartesian_waypoints_planner_B.ix_f = A->size[1];
4597 for (cartesian_waypoints_planner_B.kend = 0;
4598 cartesian_waypoints_planner_B.kend < cartesian_waypoints_planner_B.ix_f;
4599 cartesian_waypoints_planner_B.kend++) {
4600 work->data[cartesian_waypoints_planner_B.kend] = 0.0;
4601 }
4602
4603 cartesian_waypoi_emxInit_real_T(&vn1, 1);
4604 cartesian_waypoints_planner_B.kend = vn1->size[0];
4605 vn1->size[0] = A->size[1];
4606 cartes_emxEnsureCapacity_real_T(vn1, cartesian_waypoints_planner_B.kend);
4607 cartesian_waypoints_planner_B.ix_f = A->size[1];
4608 for (cartesian_waypoints_planner_B.kend = 0;
4609 cartesian_waypoints_planner_B.kend < cartesian_waypoints_planner_B.ix_f;
4610 cartesian_waypoints_planner_B.kend++) {
4611 vn1->data[cartesian_waypoints_planner_B.kend] = 0.0;
4612 }
4613
4614 cartesian_waypoi_emxInit_real_T(&vn2, 1);
4615 cartesian_waypoints_planner_B.kend = vn2->size[0];
4616 vn2->size[0] = A->size[1];
4617 cartes_emxEnsureCapacity_real_T(vn2, cartesian_waypoints_planner_B.kend);
4618 cartesian_waypoints_planner_B.ix_f = A->size[1];
4619 for (cartesian_waypoints_planner_B.kend = 0;
4620 cartesian_waypoints_planner_B.kend < cartesian_waypoints_planner_B.ix_f;
4621 cartesian_waypoints_planner_B.kend++) {
4622 vn2->data[cartesian_waypoints_planner_B.kend] = 0.0;
4623 }
4624
4625 for (cartesian_waypoints_planner_B.m_hj = 0;
4626 cartesian_waypoints_planner_B.m_hj < n;
4627 cartesian_waypoints_planner_B.m_hj++) {
4628 cartesian_waypoints_planner_B.pvt = cartesian_waypoints_planner_B.m_hj *
4629 cartesian_waypoints_planner_B.ma;
4630 cartesian_waypoints_planner_B.smax = 0.0;
4631 if (m >= 1) {
4632 if (m == 1) {
4633 cartesian_waypoints_planner_B.smax = fabs(A->
4634 data[cartesian_waypoints_planner_B.pvt]);
4635 } else {
4636 cartesian_waypoints_planner_B.scale_i = 3.3121686421112381E-170;
4637 cartesian_waypoints_planner_B.kend = cartesian_waypoints_planner_B.pvt +
4638 m;
4639 for (cartesian_waypoints_planner_B.itemp =
4640 cartesian_waypoints_planner_B.pvt + 1;
4641 cartesian_waypoints_planner_B.itemp <=
4642 cartesian_waypoints_planner_B.kend;
4643 cartesian_waypoints_planner_B.itemp++) {
4644 cartesian_waypoints_planner_B.absxk = fabs(A->
4645 data[cartesian_waypoints_planner_B.itemp - 1]);
4646 if (cartesian_waypoints_planner_B.absxk >
4647 cartesian_waypoints_planner_B.scale_i) {
4648 cartesian_waypoints_planner_B.t_l =
4649 cartesian_waypoints_planner_B.scale_i /
4650 cartesian_waypoints_planner_B.absxk;
4651 cartesian_waypoints_planner_B.smax =
4652 cartesian_waypoints_planner_B.smax *
4653 cartesian_waypoints_planner_B.t_l *
4654 cartesian_waypoints_planner_B.t_l + 1.0;
4655 cartesian_waypoints_planner_B.scale_i =
4656 cartesian_waypoints_planner_B.absxk;
4657 } else {
4658 cartesian_waypoints_planner_B.t_l =
4659 cartesian_waypoints_planner_B.absxk /
4660 cartesian_waypoints_planner_B.scale_i;
4661 cartesian_waypoints_planner_B.smax +=
4662 cartesian_waypoints_planner_B.t_l *
4663 cartesian_waypoints_planner_B.t_l;
4664 }
4665 }
4666
4667 cartesian_waypoints_planner_B.smax =
4668 cartesian_waypoints_planner_B.scale_i * sqrt
4669 (cartesian_waypoints_planner_B.smax);
4670 }
4671 }
4672
4673 vn1->data[cartesian_waypoints_planner_B.m_hj] =
4674 cartesian_waypoints_planner_B.smax;
4675 vn2->data[cartesian_waypoints_planner_B.m_hj] = vn1->
4676 data[cartesian_waypoints_planner_B.m_hj];
4677 }
4678
4679 cartesian_waypoi_emxInit_real_T(&c_x, 2);
4680 for (cartesian_waypoints_planner_B.m_hj = 0;
4681 cartesian_waypoints_planner_B.m_hj <=
4682 cartesian_waypoints_planner_B.minmn_a; cartesian_waypoints_planner_B.m_hj
4683 ++) {
4684 cartesian_waypoints_planner_B.iy_c = cartesian_waypoints_planner_B.m_hj *
4685 cartesian_waypoints_planner_B.ma;
4686 cartesian_waypoints_planner_B.ii = cartesian_waypoints_planner_B.iy_c +
4687 cartesian_waypoints_planner_B.m_hj;
4688 cartesian_waypoints_planner_B.nmi = n - cartesian_waypoints_planner_B.m_hj;
4689 cartesian_waypoints_planner_B.mmi = (m - cartesian_waypoints_planner_B.m_hj)
4690 - 1;
4691 if (cartesian_waypoints_planner_B.nmi < 1) {
4692 cartesian_waypoints_planner_B.kend = 0;
4693 } else {
4694 cartesian_waypoints_planner_B.kend = 1;
4695 if (cartesian_waypoints_planner_B.nmi > 1) {
4696 cartesian_waypoints_planner_B.ix_f = cartesian_waypoints_planner_B.m_hj;
4697 cartesian_waypoints_planner_B.smax = fabs(vn1->
4698 data[cartesian_waypoints_planner_B.m_hj]);
4699 for (cartesian_waypoints_planner_B.itemp = 2;
4700 cartesian_waypoints_planner_B.itemp <=
4701 cartesian_waypoints_planner_B.nmi;
4702 cartesian_waypoints_planner_B.itemp++) {
4703 cartesian_waypoints_planner_B.ix_f++;
4704 cartesian_waypoints_planner_B.scale_i = fabs(vn1->
4705 data[cartesian_waypoints_planner_B.ix_f]);
4706 if (cartesian_waypoints_planner_B.scale_i >
4707 cartesian_waypoints_planner_B.smax) {
4708 cartesian_waypoints_planner_B.kend =
4709 cartesian_waypoints_planner_B.itemp;
4710 cartesian_waypoints_planner_B.smax =
4711 cartesian_waypoints_planner_B.scale_i;
4712 }
4713 }
4714 }
4715 }
4716
4717 cartesian_waypoints_planner_B.pvt = (cartesian_waypoints_planner_B.m_hj +
4718 cartesian_waypoints_planner_B.kend) - 1;
4719 if (cartesian_waypoints_planner_B.pvt + 1 !=
4720 cartesian_waypoints_planner_B.m_hj + 1) {
4721 cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];
4722 c_x->size[0] = b_A->size[0];
4723 c_x->size[1] = b_A->size[1];
4724 cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);
4725 cartesian_waypoints_planner_B.ix_f = b_A->size[0] * b_A->size[1] - 1;
4726 for (cartesian_waypoints_planner_B.kend = 0;
4727 cartesian_waypoints_planner_B.kend <=
4728 cartesian_waypoints_planner_B.ix_f;
4729 cartesian_waypoints_planner_B.kend++) {
4730 c_x->data[cartesian_waypoints_planner_B.kend] = b_A->
4731 data[cartesian_waypoints_planner_B.kend];
4732 }
4733
4734 cartesian_waypoints_planner_B.ix_f = cartesian_waypoints_planner_B.pvt *
4735 cartesian_waypoints_planner_B.ma;
4736 for (cartesian_waypoints_planner_B.itemp = 0;
4737 cartesian_waypoints_planner_B.itemp < m;
4738 cartesian_waypoints_planner_B.itemp++) {
4739 cartesian_waypoints_planner_B.scale_i = c_x->
4740 data[cartesian_waypoints_planner_B.ix_f];
4741 c_x->data[cartesian_waypoints_planner_B.ix_f] = c_x->
4742 data[cartesian_waypoints_planner_B.iy_c];
4743 c_x->data[cartesian_waypoints_planner_B.iy_c] =
4744 cartesian_waypoints_planner_B.scale_i;
4745 cartesian_waypoints_planner_B.ix_f++;
4746 cartesian_waypoints_planner_B.iy_c++;
4747 }
4748
4749 cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];
4750 b_A->size[0] = c_x->size[0];
4751 b_A->size[1] = c_x->size[1];
4752 cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);
4753 cartesian_waypoints_planner_B.ix_f = c_x->size[0] * c_x->size[1] - 1;
4754 for (cartesian_waypoints_planner_B.kend = 0;
4755 cartesian_waypoints_planner_B.kend <=
4756 cartesian_waypoints_planner_B.ix_f;
4757 cartesian_waypoints_planner_B.kend++) {
4758 b_A->data[cartesian_waypoints_planner_B.kend] = c_x->
4759 data[cartesian_waypoints_planner_B.kend];
4760 }
4761
4762 cartesian_waypoints_planner_B.itemp = b_jpvt->
4763 data[cartesian_waypoints_planner_B.pvt];
4764 b_jpvt->data[cartesian_waypoints_planner_B.pvt] = b_jpvt->
4765 data[cartesian_waypoints_planner_B.m_hj];
4766 b_jpvt->data[cartesian_waypoints_planner_B.m_hj] =
4767 cartesian_waypoints_planner_B.itemp;
4768 vn1->data[cartesian_waypoints_planner_B.pvt] = vn1->
4769 data[cartesian_waypoints_planner_B.m_hj];
4770 vn2->data[cartesian_waypoints_planner_B.pvt] = vn2->
4771 data[cartesian_waypoints_planner_B.m_hj];
4772 }
4773
4774 if (cartesian_waypoints_planner_B.m_hj + 1 < m) {
4775 cartesian_waypoints_planner_B.pvt = cartesian_waypoints_planner_B.ii + 2;
4776 cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];
4777 c_x->size[0] = b_A->size[0];
4778 c_x->size[1] = b_A->size[1];
4779 cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);
4780 cartesian_waypoints_planner_B.ix_f = b_A->size[0] * b_A->size[1] - 1;
4781 for (cartesian_waypoints_planner_B.kend = 0;
4782 cartesian_waypoints_planner_B.kend <=
4783 cartesian_waypoints_planner_B.ix_f;
4784 cartesian_waypoints_planner_B.kend++) {
4785 c_x->data[cartesian_waypoints_planner_B.kend] = b_A->
4786 data[cartesian_waypoints_planner_B.kend];
4787 }
4788
4789 cartesian_waypoints_planner_B.smax = b_A->
4790 data[cartesian_waypoints_planner_B.ii];
4791 tau->data[cartesian_waypoints_planner_B.m_hj] = 0.0;
4792 if (cartesian_waypoints_planner_B.mmi + 1 > 0) {
4793 cartesian_waypoints_planner_B.scale_i = cartesian_waypoints_pl_xnrm2_ev
4794 (cartesian_waypoints_planner_B.mmi, b_A,
4795 cartesian_waypoints_planner_B.ii + 2);
4796 if (cartesian_waypoints_planner_B.scale_i != 0.0) {
4797 cartesian_waypoints_planner_B.scale_i = rt_hypotd_snf(b_A->
4798 data[cartesian_waypoints_planner_B.ii],
4799 cartesian_waypoints_planner_B.scale_i);
4800 if (b_A->data[cartesian_waypoints_planner_B.ii] >= 0.0) {
4801 cartesian_waypoints_planner_B.scale_i =
4802 -cartesian_waypoints_planner_B.scale_i;
4803 }
4804
4805 if (fabs(cartesian_waypoints_planner_B.scale_i) <
4806 1.0020841800044864E-292) {
4807 cartesian_waypoints_planner_B.kend = -1;
4808 cartesian_waypoints_planner_B.ix_f =
4809 (cartesian_waypoints_planner_B.ii +
4810 cartesian_waypoints_planner_B.mmi) + 1;
4811 do {
4812 cartesian_waypoints_planner_B.kend++;
4813 for (cartesian_waypoints_planner_B.itemp =
4814 cartesian_waypoints_planner_B.pvt;
4815 cartesian_waypoints_planner_B.itemp <=
4816 cartesian_waypoints_planner_B.ix_f;
4817 cartesian_waypoints_planner_B.itemp++) {
4818 c_x->data[cartesian_waypoints_planner_B.itemp - 1] *=
4819 9.9792015476736E+291;
4820 }
4821
4822 cartesian_waypoints_planner_B.scale_i *= 9.9792015476736E+291;
4823 cartesian_waypoints_planner_B.smax *= 9.9792015476736E+291;
4824 } while (!(fabs(cartesian_waypoints_planner_B.scale_i) >=
4825 1.0020841800044864E-292));
4826
4827 cartesian_waypoints_planner_B.scale_i = rt_hypotd_snf
4828 (cartesian_waypoints_planner_B.smax,
4829 cartesian_waypoints_pl_xnrm2_ev(cartesian_waypoints_planner_B.mmi,
4830 c_x, cartesian_waypoints_planner_B.ii + 2));
4831 if (cartesian_waypoints_planner_B.smax >= 0.0) {
4832 cartesian_waypoints_planner_B.scale_i =
4833 -cartesian_waypoints_planner_B.scale_i;
4834 }
4835
4836 tau->data[cartesian_waypoints_planner_B.m_hj] =
4837 (cartesian_waypoints_planner_B.scale_i -
4838 cartesian_waypoints_planner_B.smax) /
4839 cartesian_waypoints_planner_B.scale_i;
4840 cartesian_waypoints_planner_B.smax = 1.0 /
4841 (cartesian_waypoints_planner_B.smax -
4842 cartesian_waypoints_planner_B.scale_i);
4843 for (cartesian_waypoints_planner_B.itemp =
4844 cartesian_waypoints_planner_B.pvt;
4845 cartesian_waypoints_planner_B.itemp <=
4846 cartesian_waypoints_planner_B.ix_f;
4847 cartesian_waypoints_planner_B.itemp++) {
4848 c_x->data[cartesian_waypoints_planner_B.itemp - 1] *=
4849 cartesian_waypoints_planner_B.smax;
4850 }
4851
4852 for (cartesian_waypoints_planner_B.itemp = 0;
4853 cartesian_waypoints_planner_B.itemp <=
4854 cartesian_waypoints_planner_B.kend;
4855 cartesian_waypoints_planner_B.itemp++) {
4856 cartesian_waypoints_planner_B.scale_i *= 1.0020841800044864E-292;
4857 }
4858
4859 cartesian_waypoints_planner_B.smax =
4860 cartesian_waypoints_planner_B.scale_i;
4861 } else {
4862 tau->data[cartesian_waypoints_planner_B.m_hj] =
4863 (cartesian_waypoints_planner_B.scale_i - b_A->
4864 data[cartesian_waypoints_planner_B.ii]) /
4865 cartesian_waypoints_planner_B.scale_i;
4866 cartesian_waypoints_planner_B.smax = 1.0 / (b_A->
4867 data[cartesian_waypoints_planner_B.ii] -
4868 cartesian_waypoints_planner_B.scale_i);
4869 cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];
4870 c_x->size[0] = b_A->size[0];
4871 c_x->size[1] = b_A->size[1];
4872 cartes_emxEnsureCapacity_real_T(c_x,
4873 cartesian_waypoints_planner_B.kend);
4874 cartesian_waypoints_planner_B.ix_f = b_A->size[0] * b_A->size[1] - 1;
4875 for (cartesian_waypoints_planner_B.kend = 0;
4876 cartesian_waypoints_planner_B.kend <=
4877 cartesian_waypoints_planner_B.ix_f;
4878 cartesian_waypoints_planner_B.kend++) {
4879 c_x->data[cartesian_waypoints_planner_B.kend] = b_A->
4880 data[cartesian_waypoints_planner_B.kend];
4881 }
4882
4883 cartesian_waypoints_planner_B.b_kb =
4884 (cartesian_waypoints_planner_B.ii +
4885 cartesian_waypoints_planner_B.mmi) + 1;
4886 for (cartesian_waypoints_planner_B.itemp =
4887 cartesian_waypoints_planner_B.pvt;
4888 cartesian_waypoints_planner_B.itemp <=
4889 cartesian_waypoints_planner_B.b_kb;
4890 cartesian_waypoints_planner_B.itemp++) {
4891 c_x->data[cartesian_waypoints_planner_B.itemp - 1] *=
4892 cartesian_waypoints_planner_B.smax;
4893 }
4894
4895 cartesian_waypoints_planner_B.smax =
4896 cartesian_waypoints_planner_B.scale_i;
4897 }
4898 }
4899 }
4900
4901 cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];
4902 b_A->size[0] = c_x->size[0];
4903 b_A->size[1] = c_x->size[1];
4904 cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);
4905 cartesian_waypoints_planner_B.ix_f = c_x->size[0] * c_x->size[1] - 1;
4906 for (cartesian_waypoints_planner_B.kend = 0;
4907 cartesian_waypoints_planner_B.kend <=
4908 cartesian_waypoints_planner_B.ix_f;
4909 cartesian_waypoints_planner_B.kend++) {
4910 b_A->data[cartesian_waypoints_planner_B.kend] = c_x->
4911 data[cartesian_waypoints_planner_B.kend];
4912 }
4913
4914 b_A->data[cartesian_waypoints_planner_B.ii] =
4915 cartesian_waypoints_planner_B.smax;
4916 } else {
4917 tau->data[cartesian_waypoints_planner_B.m_hj] = 0.0;
4918 }
4919
4920 if (cartesian_waypoints_planner_B.m_hj + 1 < n) {
4921 cartesian_waypoints_planner_B.smax = b_A->
4922 data[cartesian_waypoints_planner_B.ii];
4923 b_A->data[cartesian_waypoints_planner_B.ii] = 1.0;
4924 cartesian_waypoints_planner_B.pvt = (cartesian_waypoints_planner_B.ii +
4925 cartesian_waypoints_planner_B.ma) + 1;
4926 cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];
4927 c_x->size[0] = b_A->size[0];
4928 c_x->size[1] = b_A->size[1];
4929 cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);
4930 cartesian_waypoints_planner_B.ix_f = b_A->size[0] * b_A->size[1] - 1;
4931 for (cartesian_waypoints_planner_B.kend = 0;
4932 cartesian_waypoints_planner_B.kend <=
4933 cartesian_waypoints_planner_B.ix_f;
4934 cartesian_waypoints_planner_B.kend++) {
4935 c_x->data[cartesian_waypoints_planner_B.kend] = b_A->
4936 data[cartesian_waypoints_planner_B.kend];
4937 }
4938
4939 if (tau->data[cartesian_waypoints_planner_B.m_hj] != 0.0) {
4940 cartesian_waypoints_planner_B.itemp = cartesian_waypoints_planner_B.mmi;
4941 cartesian_waypoints_planner_B.kend = cartesian_waypoints_planner_B.ii +
4942 cartesian_waypoints_planner_B.mmi;
4943 while ((cartesian_waypoints_planner_B.itemp + 1 > 0) && (b_A->
4944 data[cartesian_waypoints_planner_B.kend] == 0.0)) {
4945 cartesian_waypoints_planner_B.itemp--;
4946 cartesian_waypoints_planner_B.kend--;
4947 }
4948
4949 cartesian_waypoints_planner_B.nmi--;
4950 exitg2 = false;
4951 while ((!exitg2) && (cartesian_waypoints_planner_B.nmi > 0)) {
4952 cartesian_waypoints_planner_B.ix_f =
4953 (cartesian_waypoints_planner_B.nmi - 1) *
4954 cartesian_waypoints_planner_B.ma + cartesian_waypoints_planner_B.pvt;
4955 cartesian_waypoints_planner_B.kend =
4956 cartesian_waypoints_planner_B.ix_f;
4957 do {
4958 exitg1 = 0;
4959 if (cartesian_waypoints_planner_B.kend <=
4960 cartesian_waypoints_planner_B.ix_f +
4961 cartesian_waypoints_planner_B.itemp) {
4962 if (b_A->data[cartesian_waypoints_planner_B.kend - 1] != 0.0) {
4963 exitg1 = 1;
4964 } else {
4965 cartesian_waypoints_planner_B.kend++;
4966 }
4967 } else {
4968 cartesian_waypoints_planner_B.nmi--;
4969 exitg1 = 2;
4970 }
4971 } while (exitg1 == 0);
4972
4973 if (exitg1 == 1) {
4974 exitg2 = true;
4975 }
4976 }
4977
4978 cartesian_waypoints_planner_B.lastc = cartesian_waypoints_planner_B.nmi
4979 - 1;
4980 cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];
4981 c_x->size[0] = b_A->size[0];
4982 c_x->size[1] = b_A->size[1];
4983 cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);
4984 cartesian_waypoints_planner_B.ix_f = b_A->size[0] * b_A->size[1] - 1;
4985 for (cartesian_waypoints_planner_B.kend = 0;
4986 cartesian_waypoints_planner_B.kend <=
4987 cartesian_waypoints_planner_B.ix_f;
4988 cartesian_waypoints_planner_B.kend++) {
4989 c_x->data[cartesian_waypoints_planner_B.kend] = b_A->
4990 data[cartesian_waypoints_planner_B.kend];
4991 }
4992 } else {
4993 cartesian_waypoints_planner_B.itemp = -1;
4994 cartesian_waypoints_planner_B.lastc = -1;
4995 }
4996
4997 if (cartesian_waypoints_planner_B.itemp + 1 > 0) {
4998 if (cartesian_waypoints_planner_B.lastc + 1 != 0) {
4999 for (cartesian_waypoints_planner_B.kend = 0;
5000 cartesian_waypoints_planner_B.kend <=
5001 cartesian_waypoints_planner_B.lastc;
5002 cartesian_waypoints_planner_B.kend++) {
5003 work->data[cartesian_waypoints_planner_B.kend] = 0.0;
5004 }
5005
5006 cartesian_waypoints_planner_B.iy_c = 0;
5007 cartesian_waypoints_planner_B.b_kb = cartesian_waypoints_planner_B.ma *
5008 cartesian_waypoints_planner_B.lastc +
5009 cartesian_waypoints_planner_B.pvt;
5010 for (cartesian_waypoints_planner_B.nmi =
5011 cartesian_waypoints_planner_B.pvt;
5012 cartesian_waypoints_planner_B.ma < 0 ?
5013 cartesian_waypoints_planner_B.nmi >=
5014 cartesian_waypoints_planner_B.b_kb :
5015 cartesian_waypoints_planner_B.nmi <=
5016 cartesian_waypoints_planner_B.b_kb;
5017 cartesian_waypoints_planner_B.nmi +=
5018 cartesian_waypoints_planner_B.ma) {
5019 cartesian_waypoints_planner_B.ix_f =
5020 cartesian_waypoints_planner_B.ii;
5021 cartesian_waypoints_planner_B.scale_i = 0.0;
5022 cartesian_waypoints_planner_B.d_j =
5023 cartesian_waypoints_planner_B.nmi +
5024 cartesian_waypoints_planner_B.itemp;
5025 for (cartesian_waypoints_planner_B.kend =
5026 cartesian_waypoints_planner_B.nmi;
5027 cartesian_waypoints_planner_B.kend <=
5028 cartesian_waypoints_planner_B.d_j;
5029 cartesian_waypoints_planner_B.kend++) {
5030 cartesian_waypoints_planner_B.scale_i += c_x->
5031 data[cartesian_waypoints_planner_B.kend - 1] * c_x->
5032 data[cartesian_waypoints_planner_B.ix_f];
5033 cartesian_waypoints_planner_B.ix_f++;
5034 }
5035
5036 work->data[cartesian_waypoints_planner_B.iy_c] +=
5037 cartesian_waypoints_planner_B.scale_i;
5038 cartesian_waypoints_planner_B.iy_c++;
5039 }
5040 }
5041
5042 if (!(-tau->data[cartesian_waypoints_planner_B.m_hj] == 0.0)) {
5043 cartesian_waypoints_planner_B.iy_c = 0;
5044 for (cartesian_waypoints_planner_B.kend = 0;
5045 cartesian_waypoints_planner_B.kend <=
5046 cartesian_waypoints_planner_B.lastc;
5047 cartesian_waypoints_planner_B.kend++) {
5048 if (work->data[cartesian_waypoints_planner_B.iy_c] != 0.0) {
5049 cartesian_waypoints_planner_B.scale_i = work->
5050 data[cartesian_waypoints_planner_B.iy_c] * -tau->
5051 data[cartesian_waypoints_planner_B.m_hj];
5052 cartesian_waypoints_planner_B.ix_f =
5053 cartesian_waypoints_planner_B.ii;
5054 cartesian_waypoints_planner_B.b_kb =
5055 cartesian_waypoints_planner_B.itemp +
5056 cartesian_waypoints_planner_B.pvt;
5057 for (cartesian_waypoints_planner_B.nmi =
5058 cartesian_waypoints_planner_B.pvt;
5059 cartesian_waypoints_planner_B.nmi <=
5060 cartesian_waypoints_planner_B.b_kb;
5061 cartesian_waypoints_planner_B.nmi++) {
5062 c_x->data[cartesian_waypoints_planner_B.nmi - 1] += c_x->
5063 data[cartesian_waypoints_planner_B.ix_f] *
5064 cartesian_waypoints_planner_B.scale_i;
5065 cartesian_waypoints_planner_B.ix_f++;
5066 }
5067 }
5068
5069 cartesian_waypoints_planner_B.iy_c++;
5070 cartesian_waypoints_planner_B.pvt +=
5071 cartesian_waypoints_planner_B.ma;
5072 }
5073 }
5074 }
5075
5076 cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];
5077 b_A->size[0] = c_x->size[0];
5078 b_A->size[1] = c_x->size[1];
5079 cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);
5080 cartesian_waypoints_planner_B.ix_f = c_x->size[0] * c_x->size[1] - 1;
5081 for (cartesian_waypoints_planner_B.kend = 0;
5082 cartesian_waypoints_planner_B.kend <=
5083 cartesian_waypoints_planner_B.ix_f;
5084 cartesian_waypoints_planner_B.kend++) {
5085 b_A->data[cartesian_waypoints_planner_B.kend] = c_x->
5086 data[cartesian_waypoints_planner_B.kend];
5087 }
5088
5089 b_A->data[cartesian_waypoints_planner_B.ii] =
5090 cartesian_waypoints_planner_B.smax;
5091 }
5092
5093 for (cartesian_waypoints_planner_B.ii = cartesian_waypoints_planner_B.m_hj +
5094 2; cartesian_waypoints_planner_B.ii <= n;
5095 cartesian_waypoints_planner_B.ii++) {
5096 cartesian_waypoints_planner_B.pvt = ((cartesian_waypoints_planner_B.ii - 1)
5097 * cartesian_waypoints_planner_B.ma + cartesian_waypoints_planner_B.m_hj)
5098 + 1;
5099 if (vn1->data[cartesian_waypoints_planner_B.ii - 1] != 0.0) {
5100 cartesian_waypoints_planner_B.smax = fabs(b_A->
5101 data[cartesian_waypoints_planner_B.pvt - 1]) / vn1->
5102 data[cartesian_waypoints_planner_B.ii - 1];
5103 cartesian_waypoints_planner_B.smax = 1.0 -
5104 cartesian_waypoints_planner_B.smax *
5105 cartesian_waypoints_planner_B.smax;
5106 if (cartesian_waypoints_planner_B.smax < 0.0) {
5107 cartesian_waypoints_planner_B.smax = 0.0;
5108 }
5109
5110 cartesian_waypoints_planner_B.scale_i = vn1->
5111 data[cartesian_waypoints_planner_B.ii - 1] / vn2->
5112 data[cartesian_waypoints_planner_B.ii - 1];
5113 cartesian_waypoints_planner_B.scale_i =
5114 cartesian_waypoints_planner_B.scale_i *
5115 cartesian_waypoints_planner_B.scale_i *
5116 cartesian_waypoints_planner_B.smax;
5117 if (cartesian_waypoints_planner_B.scale_i <= 1.4901161193847656E-8) {
5118 if (cartesian_waypoints_planner_B.m_hj + 1 < m) {
5119 cartesian_waypoints_planner_B.smax = 0.0;
5120 if (cartesian_waypoints_planner_B.mmi >= 1) {
5121 if (cartesian_waypoints_planner_B.mmi == 1) {
5122 cartesian_waypoints_planner_B.smax = fabs(b_A->
5123 data[cartesian_waypoints_planner_B.pvt]);
5124 } else {
5125 cartesian_waypoints_planner_B.scale_i = 3.3121686421112381E-170;
5126 cartesian_waypoints_planner_B.kend =
5127 cartesian_waypoints_planner_B.pvt +
5128 cartesian_waypoints_planner_B.mmi;
5129 for (cartesian_waypoints_planner_B.itemp =
5130 cartesian_waypoints_planner_B.pvt + 1;
5131 cartesian_waypoints_planner_B.itemp <=
5132 cartesian_waypoints_planner_B.kend;
5133 cartesian_waypoints_planner_B.itemp++) {
5134 cartesian_waypoints_planner_B.absxk = fabs(b_A->
5135 data[cartesian_waypoints_planner_B.itemp - 1]);
5136 if (cartesian_waypoints_planner_B.absxk >
5137 cartesian_waypoints_planner_B.scale_i) {
5138 cartesian_waypoints_planner_B.t_l =
5139 cartesian_waypoints_planner_B.scale_i /
5140 cartesian_waypoints_planner_B.absxk;
5141 cartesian_waypoints_planner_B.smax =
5142 cartesian_waypoints_planner_B.smax *
5143 cartesian_waypoints_planner_B.t_l *
5144 cartesian_waypoints_planner_B.t_l + 1.0;
5145 cartesian_waypoints_planner_B.scale_i =
5146 cartesian_waypoints_planner_B.absxk;
5147 } else {
5148 cartesian_waypoints_planner_B.t_l =
5149 cartesian_waypoints_planner_B.absxk /
5150 cartesian_waypoints_planner_B.scale_i;
5151 cartesian_waypoints_planner_B.smax +=
5152 cartesian_waypoints_planner_B.t_l *
5153 cartesian_waypoints_planner_B.t_l;
5154 }
5155 }
5156
5157 cartesian_waypoints_planner_B.smax =
5158 cartesian_waypoints_planner_B.scale_i * sqrt
5159 (cartesian_waypoints_planner_B.smax);
5160 }
5161 }
5162
5163 vn1->data[cartesian_waypoints_planner_B.ii - 1] =
5164 cartesian_waypoints_planner_B.smax;
5165 vn2->data[cartesian_waypoints_planner_B.ii - 1] = vn1->
5166 data[cartesian_waypoints_planner_B.ii - 1];
5167 } else {
5168 vn1->data[cartesian_waypoints_planner_B.ii - 1] = 0.0;
5169 vn2->data[cartesian_waypoints_planner_B.ii - 1] = 0.0;
5170 }
5171 } else {
5172 vn1->data[cartesian_waypoints_planner_B.ii - 1] *= sqrt
5173 (cartesian_waypoints_planner_B.smax);
5174 }
5175 }
5176 }
5177 }
5178
5179 cartesian_waypoi_emxFree_real_T(&c_x);
5180 cartesian_waypoi_emxFree_real_T(&vn2);
5181 cartesian_waypoi_emxFree_real_T(&vn1);
5182 cartesian_waypoi_emxFree_real_T(&work);
5183}
5184
5185static void cartesian_waypoints_pla_xzgetrf(int32_T m, int32_T n, const
5186 emxArray_real_T_cartesian_way_T *A, int32_T lda,
5187 emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T *ipiv,
5188 int32_T *info)
5189{
5190 int32_T mmj;
5191 int32_T b;
5192 int32_T c;
5193 emxArray_real_T_cartesian_way_T *c_x;
5194 int32_T ix;
5195 real_T smax;
5196 real_T s;
5197 int32_T iy;
5198 int32_T n_0;
5199 int32_T yk;
5200 int32_T k;
5201 int32_T jA;
5202 int32_T jy;
5203 int32_T c_0;
5204 int32_T c_tmp;
5205 k = b_A->size[0] * b_A->size[1];
5206 b_A->size[0] = A->size[0];
5207 b_A->size[1] = A->size[1];
5208 cartes_emxEnsureCapacity_real_T(b_A, k);
5209 iy = A->size[0] * A->size[1] - 1;
5210 for (k = 0; k <= iy; k++) {
5211 b_A->data[k] = A->data[k];
5212 }
5213
5214 if (m < n) {
5215 n_0 = m;
5216 } else {
5217 n_0 = n;
5218 }
5219
5220 if (n_0 < 1) {
5221 n_0 = 0;
5222 }
5223
5224 k = ipiv->size[0] * ipiv->size[1];
5225 ipiv->size[0] = 1;
5226 ipiv->size[1] = n_0;
5227 carte_emxEnsureCapacity_int32_T(ipiv, k);
5228 if (n_0 > 0) {
5229 ipiv->data[0] = 1;
5230 yk = 1;
5231 for (k = 2; k <= n_0; k++) {
5232 yk++;
5233 ipiv->data[k - 1] = yk;
5234 }
5235 }
5236
5237 yk = 0;
5238 cartesian_waypoi_emxInit_real_T(&c_x, 2);
5239 if ((m < 1) || (n < 1)) {
5240 } else {
5241 n_0 = m - 1;
5242 if (n_0 >= n) {
5243 n_0 = n;
5244 }
5245
5246 b = n_0 - 1;
5247 for (n_0 = 0; n_0 <= b; n_0++) {
5248 mmj = m - n_0;
5249 c_tmp = (lda + 1) * n_0;
5250 c = c_tmp + 2;
5251 if (mmj < 1) {
5252 iy = 0;
5253 } else {
5254 iy = 1;
5255 if (mmj > 1) {
5256 ix = c - 2;
5257 smax = fabs(b_A->data[c_tmp]);
5258 for (k = 2; k <= mmj; k++) {
5259 ix++;
5260 s = fabs(b_A->data[ix]);
5261 if (s > smax) {
5262 iy = k;
5263 smax = s;
5264 }
5265 }
5266 }
5267 }
5268
5269 if (b_A->data[(c + iy) - 3] != 0.0) {
5270 if (iy - 1 != 0) {
5271 k = n_0 + iy;
5272 ipiv->data[n_0] = k;
5273 ix = c_x->size[0] * c_x->size[1];
5274 c_x->size[0] = b_A->size[0];
5275 c_x->size[1] = b_A->size[1];
5276 cartes_emxEnsureCapacity_real_T(c_x, ix);
5277 iy = b_A->size[0] * b_A->size[1] - 1;
5278 for (ix = 0; ix <= iy; ix++) {
5279 c_x->data[ix] = b_A->data[ix];
5280 }
5281
5282 ix = n_0;
5283 iy = k - 1;
5284 for (k = 0; k < n; k++) {
5285 smax = c_x->data[ix];
5286 c_x->data[ix] = c_x->data[iy];
5287 c_x->data[iy] = smax;
5288 ix += lda;
5289 iy += lda;
5290 }
5291
5292 k = b_A->size[0] * b_A->size[1];
5293 b_A->size[0] = c_x->size[0];
5294 b_A->size[1] = c_x->size[1];
5295 cartes_emxEnsureCapacity_real_T(b_A, k);
5296 iy = c_x->size[0] * c_x->size[1] - 1;
5297 for (k = 0; k <= iy; k++) {
5298 b_A->data[k] = c_x->data[k];
5299 }
5300 }
5301
5302 iy = c + mmj;
5303 for (k = c; k <= iy - 2; k++) {
5304 b_A->data[k - 1] /= b_A->data[c_tmp];
5305 }
5306 } else {
5307 yk = n_0 + 1;
5308 }
5309
5310 iy = (n - n_0) - 2;
5311 jy = c_tmp + lda;
5312 jA = jy + 1;
5313 for (k = 0; k <= iy; k++) {
5314 smax = b_A->data[jy];
5315 if (b_A->data[jy] != 0.0) {
5316 ix = c - 1;
5317 c_0 = (mmj + jA) - 1;
5318 for (c_tmp = jA + 1; c_tmp <= c_0; c_tmp++) {
5319 b_A->data[c_tmp - 1] += b_A->data[ix] * -smax;
5320 ix++;
5321 }
5322 }
5323
5324 jy += lda;
5325 jA += lda;
5326 }
5327 }
5328
5329 if ((yk == 0) && (m <= n) && (!(b_A->data[((m - 1) * b_A->size[0] + m) - 1]
5330 != 0.0))) {
5331 yk = m;
5332 }
5333 }
5334
5335 cartesian_waypoi_emxFree_real_T(&c_x);
5336 *info = yk;
5337}
5338
5339static void cartesian_waypoints_plann_xtrsm(int32_T m, int32_T n, const
5340 emxArray_real_T_cartesian_way_T *A, int32_T lda, const
5341 emxArray_real_T_cartesian_way_T *B, int32_T ldb,
5342 emxArray_real_T_cartesian_way_T *b_B)
5343{
5344 int32_T jBcol;
5345 int32_T kAcol;
5346 int32_T i;
5347 int32_T k;
5348 int32_T b;
5349 int32_T b_i;
5350 int32_T loop_ub;
5351 int32_T i_0;
5352 int32_T tmp;
5353 i_0 = b_B->size[0] * b_B->size[1];
5354 b_B->size[0] = B->size[0];
5355 b_B->size[1] = B->size[1];
5356 cartes_emxEnsureCapacity_real_T(b_B, i_0);
5357 loop_ub = B->size[0] * B->size[1] - 1;
5358 for (i_0 = 0; i_0 <= loop_ub; i_0++) {
5359 b_B->data[i_0] = B->data[i_0];
5360 }
5361
5362 if ((n == 0) || ((B->size[0] == 0) || (B->size[1] == 0))) {
5363 } else {
5364 for (loop_ub = 0; loop_ub < n; loop_ub++) {
5365 jBcol = ldb * loop_ub - 1;
5366 for (k = m; k >= 1; k--) {
5367 kAcol = (k - 1) * lda - 1;
5368 i_0 = k + jBcol;
5369 if (b_B->data[i_0] != 0.0) {
5370 b_B->data[i_0] /= A->data[k + kAcol];
5371 b = k - 2;
5372 for (b_i = 0; b_i <= b; b_i++) {
5373 i = b_i + 1;
5374 tmp = i + jBcol;
5375 b_B->data[tmp] -= b_B->data[i_0] * A->data[i + kAcol];
5376 }
5377 }
5378 }
5379 }
5380 }
5381}
5382
5383static void cartesian_waypo_emxFree_int32_T(emxArray_int32_T_cartesian_wa_T
5384 **pEmxArray)
5385{
5386 if (*pEmxArray != (emxArray_int32_T_cartesian_wa_T *)NULL) {
5387 if (((*pEmxArray)->data != (int32_T *)NULL) && (*pEmxArray)->canFreeData) {
5388 free((*pEmxArray)->data);
5389 }
5390
5391 free((*pEmxArray)->size);
5392 free(*pEmxArray);
5393 *pEmxArray = (emxArray_int32_T_cartesian_wa_T *)NULL;
5394 }
5395}
5396
5397static void cartesian_waypoints_pl_mldivide(const
5398 emxArray_real_T_cartesian_way_T *A, const emxArray_real_T_cartesian_way_T *B,
5399 emxArray_real_T_cartesian_way_T *Y)
5400{
5401 emxArray_real_T_cartesian_way_T *c_A;
5402 emxArray_real_T_cartesian_way_T *b_tau;
5403 emxArray_int32_T_cartesian_wa_T *b_jpvt;
5404 emxArray_real_T_cartesian_way_T *B_0;
5405 emxArray_int32_T_cartesian_wa_T *b_jpvt_0;
5406 boolean_T guard1 = false;
5407 cartesian_waypoi_emxInit_real_T(&c_A, 2);
5408 cartesian_waypoi_emxInit_real_T(&b_tau, 1);
5409 cartesian_waypo_emxInit_int32_T(&b_jpvt, 2);
5410 cartesian_waypoi_emxInit_real_T(&B_0, 2);
5411 cartesian_waypo_emxInit_int32_T(&b_jpvt_0, 2);
5412 if ((A->size[0] == 0) || (A->size[1] == 0) || ((B->size[0] == 0) || (B->size[1]
5413 == 0))) {
5414 cartesian_waypoints_planner_B.minmn = A->size[1];
5415 cartesian_waypoints_planner_B.minmana = B->size[1];
5416 cartesian_waypoints_planner_B.b_i_k = Y->size[0] * Y->size[1];
5417 Y->size[0] = cartesian_waypoints_planner_B.minmn;
5418 Y->size[1] = cartesian_waypoints_planner_B.minmana;
5419 cartes_emxEnsureCapacity_real_T(Y, cartesian_waypoints_planner_B.b_i_k);
5420 cartesian_waypoints_planner_B.minmn = cartesian_waypoints_planner_B.minmn *
5421 cartesian_waypoints_planner_B.minmana - 1;
5422 for (cartesian_waypoints_planner_B.b_i_k = 0;
5423 cartesian_waypoints_planner_B.b_i_k <=
5424 cartesian_waypoints_planner_B.minmn;
5425 cartesian_waypoints_planner_B.b_i_k++) {
5426 Y->data[cartesian_waypoints_planner_B.b_i_k] = 0.0;
5427 }
5428 } else if (A->size[0] == A->size[1]) {
5429 cartesian_waypoints_planner_B.minmn = A->size[0];
5430 cartesian_waypoints_planner_B.rankR = A->size[1];
5431 if (cartesian_waypoints_planner_B.minmn <
5432 cartesian_waypoints_planner_B.rankR) {
5433 cartesian_waypoints_planner_B.rankR = cartesian_waypoints_planner_B.minmn;
5434 }
5435
5436 cartesian_waypoints_planner_B.minmn = B->size[0];
5437 if (cartesian_waypoints_planner_B.minmn <
5438 cartesian_waypoints_planner_B.rankR) {
5439 cartesian_waypoints_planner_B.rankR = cartesian_waypoints_planner_B.minmn;
5440 }
5441
5442 cartesian_waypoints_planner_B.nb = B->size[1] - 1;
5443 cartesian_waypoints_pla_xzgetrf(cartesian_waypoints_planner_B.rankR,
5444 cartesian_waypoints_planner_B.rankR, A, A->size[0], c_A, b_jpvt,
5445 &cartesian_waypoints_planner_B.minmn);
5446 cartesian_waypoints_planner_B.b_i_k = B_0->size[0] * B_0->size[1];
5447 B_0->size[0] = B->size[0];
5448 B_0->size[1] = B->size[1];
5449 cartes_emxEnsureCapacity_real_T(B_0, cartesian_waypoints_planner_B.b_i_k);
5450 cartesian_waypoints_planner_B.minmn = B->size[0] * B->size[1] - 1;
5451 for (cartesian_waypoints_planner_B.b_i_k = 0;
5452 cartesian_waypoints_planner_B.b_i_k <=
5453 cartesian_waypoints_planner_B.minmn;
5454 cartesian_waypoints_planner_B.b_i_k++) {
5455 B_0->data[cartesian_waypoints_planner_B.b_i_k] = B->
5456 data[cartesian_waypoints_planner_B.b_i_k];
5457 }
5458
5459 cartesian_waypoints_planner_B.minmn = cartesian_waypoints_planner_B.rankR -
5460 2;
5461 for (cartesian_waypoints_planner_B.b_i_k = 0;
5462 cartesian_waypoints_planner_B.b_i_k <=
5463 cartesian_waypoints_planner_B.minmn;
5464 cartesian_waypoints_planner_B.b_i_k++) {
5465 if (cartesian_waypoints_planner_B.b_i_k + 1 != b_jpvt->
5466 data[cartesian_waypoints_planner_B.b_i_k]) {
5467 cartesian_waypoints_planner_B.na = b_jpvt->
5468 data[cartesian_waypoints_planner_B.b_i_k] - 1;
5469 for (cartesian_waypoints_planner_B.minmana = 0;
5470 cartesian_waypoints_planner_B.minmana <=
5471 cartesian_waypoints_planner_B.nb;
5472 cartesian_waypoints_planner_B.minmana++) {
5473 cartesian_waypoints_planner_B.tol_n = B_0->data[B_0->size[0] *
5474 cartesian_waypoints_planner_B.minmana +
5475 cartesian_waypoints_planner_B.b_i_k];
5476 B_0->data[cartesian_waypoints_planner_B.b_i_k + B_0->size[0] *
5477 cartesian_waypoints_planner_B.minmana] = B_0->data[B_0->size[0] *
5478 cartesian_waypoints_planner_B.minmana +
5479 cartesian_waypoints_planner_B.na];
5480 B_0->data[cartesian_waypoints_planner_B.na + B_0->size[0] *
5481 cartesian_waypoints_planner_B.minmana] =
5482 cartesian_waypoints_planner_B.tol_n;
5483 }
5484 }
5485 }
5486
5487 if ((B->size[1] == 0) || ((B_0->size[0] == 0) || (B_0->size[1] == 0))) {
5488 } else {
5489 for (cartesian_waypoints_planner_B.minmana = 0;
5490 cartesian_waypoints_planner_B.minmana <=
5491 cartesian_waypoints_planner_B.nb;
5492 cartesian_waypoints_planner_B.minmana++) {
5493 cartesian_waypoints_planner_B.m_p = B->size[0] *
5494 cartesian_waypoints_planner_B.minmana - 1;
5495 for (cartesian_waypoints_planner_B.minmn = 0;
5496 cartesian_waypoints_planner_B.minmn <
5497 cartesian_waypoints_planner_B.rankR;
5498 cartesian_waypoints_planner_B.minmn++) {
5499 cartesian_waypoints_planner_B.nb_m = c_A->size[0] *
5500 cartesian_waypoints_planner_B.minmn - 1;
5501 cartesian_waypoints_planner_B.b_i_k =
5502 (cartesian_waypoints_planner_B.minmn +
5503 cartesian_waypoints_planner_B.m_p) + 1;
5504 if (B_0->data[cartesian_waypoints_planner_B.b_i_k] != 0.0) {
5505 for (cartesian_waypoints_planner_B.na =
5506 cartesian_waypoints_planner_B.minmn + 2;
5507 cartesian_waypoints_planner_B.na <=
5508 cartesian_waypoints_planner_B.rankR;
5509 cartesian_waypoints_planner_B.na++) {
5510 cartesian_waypoints_planner_B.mn =
5511 cartesian_waypoints_planner_B.na +
5512 cartesian_waypoints_planner_B.m_p;
5513 B_0->data[cartesian_waypoints_planner_B.mn] -= B_0->
5514 data[cartesian_waypoints_planner_B.b_i_k] * c_A->
5515 data[cartesian_waypoints_planner_B.na +
5516 cartesian_waypoints_planner_B.nb_m];
5517 }
5518 }
5519 }
5520 }
5521 }
5522
5523 cartesian_waypoints_plann_xtrsm(cartesian_waypoints_planner_B.rankR, B->
5524 size[1], c_A, c_A->size[0], B_0, B->size[0], Y);
5525 } else {
5526 cartesian_waypoints_planner_B.na = A->size[1] - 1;
5527 cartesian_waypoints_planner_B.b_i_k = c_A->size[0] * c_A->size[1];
5528 c_A->size[0] = A->size[0];
5529 c_A->size[1] = A->size[1];
5530 cartes_emxEnsureCapacity_real_T(c_A, cartesian_waypoints_planner_B.b_i_k);
5531 cartesian_waypoints_planner_B.minmn = A->size[0] * A->size[1] - 1;
5532 for (cartesian_waypoints_planner_B.b_i_k = 0;
5533 cartesian_waypoints_planner_B.b_i_k <=
5534 cartesian_waypoints_planner_B.minmn;
5535 cartesian_waypoints_planner_B.b_i_k++) {
5536 c_A->data[cartesian_waypoints_planner_B.b_i_k] = A->
5537 data[cartesian_waypoints_planner_B.b_i_k];
5538 }
5539
5540 cartesian_waypoints_planner_B.minmn = A->size[0];
5541 cartesian_waypoints_planner_B.minmana = A->size[1];
5542 if (cartesian_waypoints_planner_B.minmn <
5543 cartesian_waypoints_planner_B.minmana) {
5544 cartesian_waypoints_planner_B.minmana =
5545 cartesian_waypoints_planner_B.minmn;
5546 }
5547
5548 cartesian_waypoints_planner_B.b_i_k = b_tau->size[0];
5549 b_tau->size[0] = cartesian_waypoints_planner_B.minmana;
5550 cartes_emxEnsureCapacity_real_T(b_tau, cartesian_waypoints_planner_B.b_i_k);
5551 for (cartesian_waypoints_planner_B.b_i_k = 0;
5552 cartesian_waypoints_planner_B.b_i_k <
5553 cartesian_waypoints_planner_B.minmana;
5554 cartesian_waypoints_planner_B.b_i_k++) {
5555 b_tau->data[cartesian_waypoints_planner_B.b_i_k] = 0.0;
5556 }
5557
5558 guard1 = false;
5559 if ((A->size[0] == 0) || (A->size[1] == 0)) {
5560 guard1 = true;
5561 } else {
5562 cartesian_waypoints_planner_B.minmn = A->size[0];
5563 cartesian_waypoints_planner_B.minmana = A->size[1];
5564 if (cartesian_waypoints_planner_B.minmn <
5565 cartesian_waypoints_planner_B.minmana) {
5566 cartesian_waypoints_planner_B.minmana =
5567 cartesian_waypoints_planner_B.minmn;
5568 }
5569
5570 if (cartesian_waypoints_planner_B.minmana < 1) {
5571 guard1 = true;
5572 } else {
5573 cartesian_waypoints_planner_B.b_i_k = b_jpvt->size[0] * b_jpvt->size[1];
5574 b_jpvt->size[0] = 1;
5575 b_jpvt->size[1] = A->size[1];
5576 carte_emxEnsureCapacity_int32_T(b_jpvt,
5577 cartesian_waypoints_planner_B.b_i_k);
5578 cartesian_waypoints_planner_B.minmn = A->size[1] - 1;
5579 for (cartesian_waypoints_planner_B.b_i_k = 0;
5580 cartesian_waypoints_planner_B.b_i_k <=
5581 cartesian_waypoints_planner_B.minmn;
5582 cartesian_waypoints_planner_B.b_i_k++) {
5583 b_jpvt->data[cartesian_waypoints_planner_B.b_i_k] = 0;
5584 }
5585
5586 for (cartesian_waypoints_planner_B.minmn = 0;
5587 cartesian_waypoints_planner_B.minmn <=
5588 cartesian_waypoints_planner_B.na;
5589 cartesian_waypoints_planner_B.minmn++) {
5590 b_jpvt->data[cartesian_waypoints_planner_B.minmn] =
5591 cartesian_waypoints_planner_B.minmn + 1;
5592 }
5593
5594 cartesian_waypoints_planner_B.b_i_k = b_jpvt_0->size[0] * b_jpvt_0->
5595 size[1];
5596 b_jpvt_0->size[0] = 1;
5597 b_jpvt_0->size[1] = b_jpvt->size[1];
5598 carte_emxEnsureCapacity_int32_T(b_jpvt_0,
5599 cartesian_waypoints_planner_B.b_i_k);
5600 cartesian_waypoints_planner_B.minmn = b_jpvt->size[0] * b_jpvt->size[1];
5601 for (cartesian_waypoints_planner_B.b_i_k = 0;
5602 cartesian_waypoints_planner_B.b_i_k <
5603 cartesian_waypoints_planner_B.minmn;
5604 cartesian_waypoints_planner_B.b_i_k++) {
5605 b_jpvt_0->data[cartesian_waypoints_planner_B.b_i_k] = b_jpvt->
5606 data[cartesian_waypoints_planner_B.b_i_k];
5607 }
5608
5609 cartesian_waypoints_plan_qrpf_e(A, A->size[0], A->size[1], b_tau,
5610 b_jpvt_0, c_A, b_jpvt);
5611 }
5612 }
5613
5614 if (guard1) {
5615 cartesian_waypoints_planner_B.b_i_k = b_jpvt->size[0] * b_jpvt->size[1];
5616 b_jpvt->size[0] = 1;
5617 b_jpvt->size[1] = A->size[1];
5618 carte_emxEnsureCapacity_int32_T(b_jpvt,
5619 cartesian_waypoints_planner_B.b_i_k);
5620 cartesian_waypoints_planner_B.minmn = A->size[1] - 1;
5621 for (cartesian_waypoints_planner_B.b_i_k = 0;
5622 cartesian_waypoints_planner_B.b_i_k <=
5623 cartesian_waypoints_planner_B.minmn;
5624 cartesian_waypoints_planner_B.b_i_k++) {
5625 b_jpvt->data[cartesian_waypoints_planner_B.b_i_k] = 0;
5626 }
5627
5628 for (cartesian_waypoints_planner_B.minmana = 0;
5629 cartesian_waypoints_planner_B.minmana <=
5630 cartesian_waypoints_planner_B.na;
5631 cartesian_waypoints_planner_B.minmana++) {
5632 b_jpvt->data[cartesian_waypoints_planner_B.minmana] =
5633 cartesian_waypoints_planner_B.minmana + 1;
5634 }
5635 }
5636
5637 cartesian_waypoints_planner_B.rankR = 0;
5638 if (c_A->size[0] < c_A->size[1]) {
5639 cartesian_waypoints_planner_B.minmn = c_A->size[0];
5640 cartesian_waypoints_planner_B.minmana = c_A->size[1];
5641 } else {
5642 cartesian_waypoints_planner_B.minmn = c_A->size[1];
5643 cartesian_waypoints_planner_B.minmana = c_A->size[0];
5644 }
5645
5646 if (cartesian_waypoints_planner_B.minmn > 0) {
5647 cartesian_waypoints_planner_B.tol_n = 2.2204460492503131E-15 *
5648 static_cast<real_T>(cartesian_waypoints_planner_B.minmana);
5649 if (1.4901161193847656E-8 < cartesian_waypoints_planner_B.tol_n) {
5650 cartesian_waypoints_planner_B.tol_n = 1.4901161193847656E-8;
5651 }
5652
5653 cartesian_waypoints_planner_B.tol_n *= fabs(c_A->data[0]);
5654 while ((cartesian_waypoints_planner_B.rankR <
5655 cartesian_waypoints_planner_B.minmn) && (!(fabs(c_A->data
5656 [c_A->size[0] * cartesian_waypoints_planner_B.rankR +
5657 cartesian_waypoints_planner_B.rankR]) <=
5658 cartesian_waypoints_planner_B.tol_n))) {
5659 cartesian_waypoints_planner_B.rankR++;
5660 }
5661 }
5662
5663 cartesian_waypoints_planner_B.nb = B->size[1] - 1;
5664 cartesian_waypoints_planner_B.minmn = c_A->size[1];
5665 cartesian_waypoints_planner_B.minmana = B->size[1];
5666 cartesian_waypoints_planner_B.b_i_k = Y->size[0] * Y->size[1];
5667 Y->size[0] = cartesian_waypoints_planner_B.minmn;
5668 Y->size[1] = cartesian_waypoints_planner_B.minmana;
5669 cartes_emxEnsureCapacity_real_T(Y, cartesian_waypoints_planner_B.b_i_k);
5670 cartesian_waypoints_planner_B.minmn = cartesian_waypoints_planner_B.minmn *
5671 cartesian_waypoints_planner_B.minmana - 1;
5672 for (cartesian_waypoints_planner_B.b_i_k = 0;
5673 cartesian_waypoints_planner_B.b_i_k <=
5674 cartesian_waypoints_planner_B.minmn;
5675 cartesian_waypoints_planner_B.b_i_k++) {
5676 Y->data[cartesian_waypoints_planner_B.b_i_k] = 0.0;
5677 }
5678
5679 cartesian_waypoints_planner_B.b_i_k = B_0->size[0] * B_0->size[1];
5680 B_0->size[0] = B->size[0];
5681 B_0->size[1] = B->size[1];
5682 cartes_emxEnsureCapacity_real_T(B_0, cartesian_waypoints_planner_B.b_i_k);
5683 cartesian_waypoints_planner_B.minmn = B->size[0] * B->size[1] - 1;
5684 for (cartesian_waypoints_planner_B.b_i_k = 0;
5685 cartesian_waypoints_planner_B.b_i_k <=
5686 cartesian_waypoints_planner_B.minmn;
5687 cartesian_waypoints_planner_B.b_i_k++) {
5688 B_0->data[cartesian_waypoints_planner_B.b_i_k] = B->
5689 data[cartesian_waypoints_planner_B.b_i_k];
5690 }
5691
5692 cartesian_waypoints_planner_B.m_p = c_A->size[0];
5693 cartesian_waypoints_planner_B.nb_m = B->size[1] - 1;
5694 cartesian_waypoints_planner_B.minmn = c_A->size[0];
5695 cartesian_waypoints_planner_B.minmana = c_A->size[1];
5696 if (cartesian_waypoints_planner_B.minmn <
5697 cartesian_waypoints_planner_B.minmana) {
5698 cartesian_waypoints_planner_B.minmana =
5699 cartesian_waypoints_planner_B.minmn;
5700 }
5701
5702 cartesian_waypoints_planner_B.mn = cartesian_waypoints_planner_B.minmana - 1;
5703 for (cartesian_waypoints_planner_B.minmana = 0;
5704 cartesian_waypoints_planner_B.minmana <=
5705 cartesian_waypoints_planner_B.mn; cartesian_waypoints_planner_B.minmana
5706 ++) {
5707 if (b_tau->data[cartesian_waypoints_planner_B.minmana] != 0.0) {
5708 for (cartesian_waypoints_planner_B.minmn = 0;
5709 cartesian_waypoints_planner_B.minmn <=
5710 cartesian_waypoints_planner_B.nb_m;
5711 cartesian_waypoints_planner_B.minmn++) {
5712 cartesian_waypoints_planner_B.tol_n = B_0->data[B_0->size[0] *
5713 cartesian_waypoints_planner_B.minmn +
5714 cartesian_waypoints_planner_B.minmana];
5715 for (cartesian_waypoints_planner_B.na =
5716 cartesian_waypoints_planner_B.minmana + 2;
5717 cartesian_waypoints_planner_B.na <=
5718 cartesian_waypoints_planner_B.m_p;
5719 cartesian_waypoints_planner_B.na++) {
5720 cartesian_waypoints_planner_B.tol_n += c_A->data[(c_A->size[0] *
5721 cartesian_waypoints_planner_B.minmana +
5722 cartesian_waypoints_planner_B.na) - 1] * B_0->data[(B_0->size[0] *
5723 cartesian_waypoints_planner_B.minmn +
5724 cartesian_waypoints_planner_B.na) - 1];
5725 }
5726
5727 cartesian_waypoints_planner_B.tol_n *= b_tau->
5728 data[cartesian_waypoints_planner_B.minmana];
5729 if (cartesian_waypoints_planner_B.tol_n != 0.0) {
5730 B_0->data[cartesian_waypoints_planner_B.minmana + B_0->size[0] *
5731 cartesian_waypoints_planner_B.minmn] -=
5732 cartesian_waypoints_planner_B.tol_n;
5733 for (cartesian_waypoints_planner_B.b_i_k =
5734 cartesian_waypoints_planner_B.minmana + 2;
5735 cartesian_waypoints_planner_B.b_i_k <=
5736 cartesian_waypoints_planner_B.m_p;
5737 cartesian_waypoints_planner_B.b_i_k++) {
5738 B_0->data[(cartesian_waypoints_planner_B.b_i_k + B_0->size[0] *
5739 cartesian_waypoints_planner_B.minmn) - 1] -= c_A->data
5740 [(c_A->size[0] * cartesian_waypoints_planner_B.minmana +
5741 cartesian_waypoints_planner_B.b_i_k) - 1] *
5742 cartesian_waypoints_planner_B.tol_n;
5743 }
5744 }
5745 }
5746 }
5747 }
5748
5749 for (cartesian_waypoints_planner_B.minmn = 0;
5750 cartesian_waypoints_planner_B.minmn <= cartesian_waypoints_planner_B.nb;
5751 cartesian_waypoints_planner_B.minmn++) {
5752 for (cartesian_waypoints_planner_B.b_i_k = 0;
5753 cartesian_waypoints_planner_B.b_i_k <
5754 cartesian_waypoints_planner_B.rankR;
5755 cartesian_waypoints_planner_B.b_i_k++) {
5756 Y->data[(b_jpvt->data[cartesian_waypoints_planner_B.b_i_k] + Y->size[0] *
5757 cartesian_waypoints_planner_B.minmn) - 1] = B_0->data[B_0->
5758 size[0] * cartesian_waypoints_planner_B.minmn +
5759 cartesian_waypoints_planner_B.b_i_k];
5760 }
5761
5762 for (cartesian_waypoints_planner_B.minmana =
5763 cartesian_waypoints_planner_B.rankR;
5764 cartesian_waypoints_planner_B.minmana >= 1;
5765 cartesian_waypoints_planner_B.minmana--) {
5766 Y->data[(b_jpvt->data[cartesian_waypoints_planner_B.minmana - 1] +
5767 Y->size[0] * cartesian_waypoints_planner_B.minmn) - 1] /=
5768 c_A->data[((cartesian_waypoints_planner_B.minmana - 1) * c_A->size[0]
5769 + cartesian_waypoints_planner_B.minmana) - 1];
5770 cartesian_waypoints_planner_B.na = cartesian_waypoints_planner_B.minmana
5771 - 2;
5772 for (cartesian_waypoints_planner_B.b_i_k = 0;
5773 cartesian_waypoints_planner_B.b_i_k <=
5774 cartesian_waypoints_planner_B.na;
5775 cartesian_waypoints_planner_B.b_i_k++) {
5776 Y->data[(b_jpvt->data[cartesian_waypoints_planner_B.b_i_k] + Y->size[0]
5777 * cartesian_waypoints_planner_B.minmn) - 1] -= Y->data
5778 [(b_jpvt->data[cartesian_waypoints_planner_B.minmana - 1] + Y->size
5779 [0] * cartesian_waypoints_planner_B.minmn) - 1] * c_A->data
5780 [(cartesian_waypoints_planner_B.minmana - 1) * c_A->size[0] +
5781 cartesian_waypoints_planner_B.b_i_k];
5782 }
5783 }
5784 }
5785 }
5786
5787 cartesian_waypo_emxFree_int32_T(&b_jpvt_0);
5788 cartesian_waypoi_emxFree_real_T(&B_0);
5789 cartesian_waypo_emxFree_int32_T(&b_jpvt);
5790 cartesian_waypoi_emxFree_real_T(&b_tau);
5791 cartesian_waypoi_emxFree_real_T(&c_A);
5792}
5793
5794static void cartesian_way_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
5795 **pEmxArray)
5796{
5797 if (*pEmxArray != (emxArray_boolean_T_cartesian__T *)NULL) {
5798 if (((*pEmxArray)->data != (boolean_T *)NULL) && (*pEmxArray)->canFreeData)
5799 {
5800 free((*pEmxArray)->data);
5801 }
5802
5803 free((*pEmxArray)->size);
5804 free(*pEmxArray);
5805 *pEmxArray = (emxArray_boolean_T_cartesian__T *)NULL;
5806 }
5807}
5808
5809static boolean_T DampedBFGSwGradientProjection_a(const
5810 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
5811 emxArray_real_T_cartesian_way_T *alpha)
5812{
5813 boolean_T flag;
5814 boolean_T y;
5815 emxArray_boolean_T_cartesian__T *x;
5816 int32_T ix;
5817 int32_T loop_ub;
5818 boolean_T exitg1;
5819 cartesian_way_emxInit_boolean_T(&x, 1);
5820 if (cartesian_waypoints_plan_norm_e(Hg) < obj->GradientTolerance) {
5821 ix = x->size[0];
5822 x->size[0] = alpha->size[0];
5823 car_emxEnsureCapacity_boolean_T(x, ix);
5824 loop_ub = alpha->size[0];
5825 for (ix = 0; ix < loop_ub; ix++) {
5826 x->data[ix] = (alpha->data[ix] <= 0.0);
5827 }
5828
5829 y = true;
5830 ix = 0;
5831 exitg1 = false;
5832 while ((!exitg1) && (ix + 1 <= x->size[0])) {
5833 if (!x->data[ix]) {
5834 y = false;
5835 exitg1 = true;
5836 } else {
5837 ix++;
5838 }
5839 }
5840
5841 if (y) {
5842 flag = true;
5843 } else {
5844 flag = false;
5845 }
5846 } else {
5847 flag = false;
5848 }
5849
5850 cartesian_way_emxFree_boolean_T(&x);
5851 return flag;
5852}
5853
5854static void cartesian_waypoints_planner_inv(const
5855 emxArray_real_T_cartesian_way_T *x, emxArray_real_T_cartesian_way_T *y)
5856{
5857 int32_T n;
5858 emxArray_int32_T_cartesian_wa_T *p;
5859 int32_T c;
5860 emxArray_real_T_cartesian_way_T *c_A;
5861 emxArray_int32_T_cartesian_wa_T *b_ipiv;
5862 int32_T info;
5863 int32_T n_0;
5864 int32_T yk;
5865 emxArray_real_T_cartesian_way_T *y_0;
5866 if ((x->size[0] == 0) || (x->size[1] == 0)) {
5867 info = y->size[0] * y->size[1];
5868 y->size[0] = x->size[0];
5869 y->size[1] = x->size[1];
5870 cartes_emxEnsureCapacity_real_T(y, info);
5871 n_0 = x->size[0] * x->size[1] - 1;
5872 for (info = 0; info <= n_0; info++) {
5873 y->data[info] = x->data[info];
5874 }
5875 } else {
5876 n = x->size[0];
5877 info = y->size[0] * y->size[1];
5878 y->size[0] = x->size[0];
5879 y->size[1] = x->size[1];
5880 cartes_emxEnsureCapacity_real_T(y, info);
5881 n_0 = x->size[0] * x->size[1] - 1;
5882 for (info = 0; info <= n_0; info++) {
5883 y->data[info] = 0.0;
5884 }
5885
5886 cartesian_waypo_emxInit_int32_T(&p, 2);
5887 cartesian_waypoi_emxInit_real_T(&c_A, 2);
5888 cartesian_waypo_emxInit_int32_T(&b_ipiv, 2);
5889 cartesian_waypoints_pla_xzgetrf(x->size[0], x->size[0], x, x->size[0], c_A,
5890 b_ipiv, &info);
5891 if (x->size[0] < 1) {
5892 n_0 = 0;
5893 } else {
5894 n_0 = x->size[0];
5895 }
5896
5897 info = p->size[0] * p->size[1];
5898 p->size[0] = 1;
5899 p->size[1] = n_0;
5900 carte_emxEnsureCapacity_int32_T(p, info);
5901 if (n_0 > 0) {
5902 p->data[0] = 1;
5903 yk = 1;
5904 for (info = 2; info <= n_0; info++) {
5905 yk++;
5906 p->data[info - 1] = yk;
5907 }
5908 }
5909
5910 n_0 = b_ipiv->size[1] - 1;
5911 for (info = 0; info <= n_0; info++) {
5912 if (b_ipiv->data[info] > static_cast<real_T>(info) + 1.0) {
5913 yk = p->data[b_ipiv->data[info] - 1];
5914 p->data[b_ipiv->data[info] - 1] = p->data[info];
5915 p->data[info] = yk;
5916 }
5917 }
5918
5919 cartesian_waypo_emxFree_int32_T(&b_ipiv);
5920 for (info = 0; info < n; info++) {
5921 c = p->data[info] - 1;
5922 y->data[info + y->size[0] * (p->data[info] - 1)] = 1.0;
5923 for (n_0 = info + 1; n_0 <= n; n_0++) {
5924 if (y->data[(y->size[0] * c + n_0) - 1] != 0.0) {
5925 for (yk = n_0 + 1; yk <= n; yk++) {
5926 y->data[(yk + y->size[0] * c) - 1] -= c_A->data[((n_0 - 1) *
5927 c_A->size[0] + yk) - 1] * y->data[(y->size[0] * c + n_0) - 1];
5928 }
5929 }
5930 }
5931 }
5932
5933 cartesian_waypo_emxFree_int32_T(&p);
5934 cartesian_waypoi_emxInit_real_T(&y_0, 2);
5935 info = y_0->size[0] * y_0->size[1];
5936 y_0->size[0] = y->size[0];
5937 y_0->size[1] = y->size[1];
5938 cartes_emxEnsureCapacity_real_T(y_0, info);
5939 n_0 = y->size[0] * y->size[1];
5940 for (info = 0; info < n_0; info++) {
5941 y_0->data[info] = y->data[info];
5942 }
5943
5944 cartesian_waypoints_plann_xtrsm(x->size[0], x->size[0], c_A, x->size[0], y_0,
5945 x->size[0], y);
5946 cartesian_waypoi_emxFree_real_T(&y_0);
5947 cartesian_waypoi_emxFree_real_T(&c_A);
5948 }
5949}
5950
5951static void cartesian_waypoints_planne_diag(const
5952 emxArray_real_T_cartesian_way_T *v, emxArray_real_T_cartesian_way_T *d)
5953{
5954 int32_T u0;
5955 int32_T u1;
5956 if ((v->size[0] == 1) && (v->size[1] == 1)) {
5957 u0 = d->size[0];
5958 d->size[0] = 1;
5959 cartes_emxEnsureCapacity_real_T(d, u0);
5960 d->data[0] = v->data[0];
5961 } else {
5962 if (0 < v->size[1]) {
5963 u0 = v->size[0];
5964 u1 = v->size[1];
5965 if (u0 < u1) {
5966 u1 = u0;
5967 }
5968 } else {
5969 u1 = 0;
5970 }
5971
5972 u0 = d->size[0];
5973 d->size[0] = u1;
5974 cartes_emxEnsureCapacity_real_T(d, u0);
5975 for (u0 = 0; u0 < u1; u0++) {
5976 d->data[u0] = v->data[v->size[0] * u0 + u0];
5977 }
5978 }
5979}
5980
5981static void cartesian_waypoints_pla_sqrt_ev(emxArray_real_T_cartesian_way_T *x)
5982{
5983 int32_T nx;
5984 int32_T b_k;
5985 nx = x->size[0] - 1;
5986 for (b_k = 0; b_k <= nx; b_k++) {
5987 x->data[b_k] = sqrt(x->data[b_k]);
5988 }
5989}
5990
5991static boolean_T cartesian_waypoints_planner_any(const
5992 emxArray_boolean_T_cartesian__T *x)
5993{
5994 boolean_T y;
5995 int32_T ix;
5996 boolean_T exitg1;
5997 y = false;
5998 ix = 0;
5999 exitg1 = false;
6000 while ((!exitg1) && (ix + 1 <= x->size[0])) {
6001 if (!x->data[ix]) {
6002 ix++;
6003 } else {
6004 y = true;
6005 exitg1 = true;
6006 }
6007 }
6008
6009 return y;
6010}
6011
6012static boolean_T cartesian_wa_isPositiveDefinite(const real_T B[36])
6013{
6014 emxArray_real_T_cartesian_way_T *b_x;
6015 boolean_T exitg1;
6016 cartesian_waypoints_planner_B.c_A_size_idx_0 = 6;
6017 cartesian_waypoints_planner_B.c_A_size_idx_1 = 6;
6018 memcpy(&cartesian_waypoints_planner_B.c_A_data[0], &B[0], 36U * sizeof(real_T));
6019 cartesian_waypoints_planner_B.b_info = 0;
6020 cartesian_waypoints_planner_B.b_j_m = 1;
6021 cartesian_waypoi_emxInit_real_T(&b_x, 2);
6022 exitg1 = false;
6023 while ((!exitg1) && (cartesian_waypoints_planner_B.b_j_m - 1 < 6)) {
6024 cartesian_waypoints_planner_B.jm1 = cartesian_waypoints_planner_B.b_j_m - 2;
6025 cartesian_waypoints_planner_B.idxAjj = ((cartesian_waypoints_planner_B.b_j_m
6026 - 1) * 6 + cartesian_waypoints_planner_B.b_j_m) - 1;
6027 cartesian_waypoints_planner_B.ssq = 0.0;
6028 if (cartesian_waypoints_planner_B.b_j_m - 1 >= 1) {
6029 cartesian_waypoints_planner_B.ix_g = cartesian_waypoints_planner_B.b_j_m -
6030 1;
6031 cartesian_waypoints_planner_B.iy = cartesian_waypoints_planner_B.b_j_m - 1;
6032 for (cartesian_waypoints_planner_B.k_e = 0;
6033 cartesian_waypoints_planner_B.k_e <=
6034 cartesian_waypoints_planner_B.jm1; cartesian_waypoints_planner_B.k_e
6035 ++) {
6036 cartesian_waypoints_planner_B.ssq +=
6037 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_g]
6038 * cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.iy];
6039 cartesian_waypoints_planner_B.ix_g += 6;
6040 cartesian_waypoints_planner_B.iy += 6;
6041 }
6042 }
6043
6044 cartesian_waypoints_planner_B.ssq =
6045 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.idxAjj]
6046 - cartesian_waypoints_planner_B.ssq;
6047 if (cartesian_waypoints_planner_B.ssq > 0.0) {
6048 cartesian_waypoints_planner_B.ssq = sqrt(cartesian_waypoints_planner_B.ssq);
6049 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.idxAjj]
6050 = cartesian_waypoints_planner_B.ssq;
6051 if (cartesian_waypoints_planner_B.b_j_m < 6) {
6052 if (cartesian_waypoints_planner_B.b_j_m - 1 != 0) {
6053 cartesian_waypoints_planner_B.ix_g =
6054 cartesian_waypoints_planner_B.b_j_m - 1;
6055 cartesian_waypoints_planner_B.jm1 =
6056 (cartesian_waypoints_planner_B.b_j_m - 2) * 6 +
6057 cartesian_waypoints_planner_B.b_j_m;
6058 for (cartesian_waypoints_planner_B.k_e =
6059 cartesian_waypoints_planner_B.b_j_m + 1;
6060 cartesian_waypoints_planner_B.k_e <=
6061 cartesian_waypoints_planner_B.jm1 + 1;
6062 cartesian_waypoints_planner_B.k_e += 6) {
6063 cartesian_waypoints_planner_B.c =
6064 -cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_g];
6065 cartesian_waypoints_planner_B.iy =
6066 cartesian_waypoints_planner_B.idxAjj + 1;
6067 cartesian_waypoints_planner_B.d_o =
6068 cartesian_waypoints_planner_B.k_e -
6069 cartesian_waypoints_planner_B.b_j_m;
6070 for (cartesian_waypoints_planner_B.ia =
6071 cartesian_waypoints_planner_B.k_e;
6072 cartesian_waypoints_planner_B.ia <=
6073 cartesian_waypoints_planner_B.d_o + 5;
6074 cartesian_waypoints_planner_B.ia++) {
6075 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.iy]
6076 +=
6077 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ia
6078 - 1] * cartesian_waypoints_planner_B.c;
6079 cartesian_waypoints_planner_B.iy++;
6080 }
6081
6082 cartesian_waypoints_planner_B.ix_g += 6;
6083 }
6084 }
6085
6086 cartesian_waypoints_planner_B.ssq = 1.0 /
6087 cartesian_waypoints_planner_B.ssq;
6088 cartesian_waypoints_planner_B.ix_g = b_x->size[0] * b_x->size[1];
6089 b_x->size[0] = 6;
6090 b_x->size[1] = 6;
6091 cartes_emxEnsureCapacity_real_T(b_x, cartesian_waypoints_planner_B.ix_g);
6092 cartesian_waypoints_planner_B.c_A_size_idx_0 =
6093 cartesian_waypoints_planner_B.c_A_size_idx_0 *
6094 cartesian_waypoints_planner_B.c_A_size_idx_1 - 1;
6095 for (cartesian_waypoints_planner_B.ix_g = 0;
6096 cartesian_waypoints_planner_B.ix_g <=
6097 cartesian_waypoints_planner_B.c_A_size_idx_0;
6098 cartesian_waypoints_planner_B.ix_g++) {
6099 b_x->data[cartesian_waypoints_planner_B.ix_g] =
6100 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_g];
6101 }
6102
6103 cartesian_waypoints_planner_B.jm1 = cartesian_waypoints_planner_B.idxAjj
6104 - cartesian_waypoints_planner_B.b_j_m;
6105 for (cartesian_waypoints_planner_B.k_e =
6106 cartesian_waypoints_planner_B.idxAjj + 2;
6107 cartesian_waypoints_planner_B.k_e <=
6108 cartesian_waypoints_planner_B.jm1 + 7;
6109 cartesian_waypoints_planner_B.k_e++) {
6110 b_x->data[cartesian_waypoints_planner_B.k_e - 1] *=
6111 cartesian_waypoints_planner_B.ssq;
6112 }
6113
6114 cartesian_waypoints_planner_B.c_A_size_idx_0 = b_x->size[0];
6115 cartesian_waypoints_planner_B.c_A_size_idx_1 = b_x->size[1];
6116 for (cartesian_waypoints_planner_B.ix_g = 0;
6117 cartesian_waypoints_planner_B.ix_g < 36;
6118 cartesian_waypoints_planner_B.ix_g++) {
6119 cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_g]
6120 = b_x->data[cartesian_waypoints_planner_B.ix_g];
6121 }
6122 }
6123
6124 cartesian_waypoints_planner_B.b_j_m++;
6125 } else {
6126 cartesian_waypoints_planner_B.b_info = cartesian_waypoints_planner_B.b_j_m;
6127 exitg1 = true;
6128 }
6129 }
6130
6131 cartesian_waypoi_emxFree_real_T(&b_x);
6132 return cartesian_waypoints_planner_B.b_info == 0;
6133}
6134
6135static boolean_T DampedBFGSwGradientProjection_e(const
6136 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6])
6137{
6138 boolean_T flag;
6139 emxArray_real_T_cartesian_way_T *b;
6140 emxArray_real_T_cartesian_way_T *c;
6141 int32_T m;
6142 int32_T inner;
6143 int32_T b_i;
6144 int32_T loop_ub;
6145 emxArray_boolean_T_cartesian__T *c_0;
6146 cartesian_waypoi_emxInit_real_T(&b, 2);
6147 cartesian_waypoi_emxInit_real_T(&c, 1);
6148 cartesian_way_emxInit_boolean_T(&c_0, 1);
6149 if (obj->ConstraintsOn) {
6150 b_i = b->size[0] * b->size[1];
6151 b->size[0] = obj->ConstraintMatrix->size[0];
6152 b->size[1] = obj->ConstraintMatrix->size[1];
6153 cartes_emxEnsureCapacity_real_T(b, b_i);
6154 loop_ub = obj->ConstraintMatrix->size[0] * obj->ConstraintMatrix->size[1] -
6155 1;
6156 for (b_i = 0; b_i <= loop_ub; b_i++) {
6157 b->data[b_i] = obj->ConstraintMatrix->data[b_i];
6158 }
6159
6160 m = b->size[1] - 1;
6161 inner = b->size[0] - 1;
6162 b_i = c->size[0];
6163 c->size[0] = b->size[1];
6164 cartes_emxEnsureCapacity_real_T(c, b_i);
6165 for (b_i = 0; b_i <= m; b_i++) {
6166 c->data[b_i] = 0.0;
6167 }
6168
6169 for (b_i = 0; b_i <= inner; b_i++) {
6170 for (loop_ub = 0; loop_ub <= m; loop_ub++) {
6171 c->data[loop_ub] += b->data[loop_ub * b->size[0] + b_i] * xNew[b_i];
6172 }
6173 }
6174
6175 b_i = c_0->size[0];
6176 c_0->size[0] = c->size[0];
6177 car_emxEnsureCapacity_boolean_T(c_0, b_i);
6178 loop_ub = c->size[0];
6179 for (b_i = 0; b_i < loop_ub; b_i++) {
6180 c_0->data[b_i] = (c->data[b_i] - obj->ConstraintBound->data[b_i] >
6181 1.4901161193847656E-8);
6182 }
6183
6184 if (cartesian_waypoints_planner_any(c_0)) {
6185 flag = true;
6186 } else {
6187 flag = false;
6188 }
6189 } else {
6190 flag = false;
6191 }
6192
6193 cartesian_way_emxFree_boolean_T(&c_0);
6194 cartesian_waypoi_emxFree_real_T(&c);
6195 cartesian_waypoi_emxFree_real_T(&b);
6196 return flag;
6197}
6198
6199static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
6200 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
6201 *iter)
6202{
6203 emxArray_real_T_cartesian_way_T *unusedU1;
6204 emxArray_real_T_cartesian_way_T *grad;
6205 emxArray_boolean_T_cartesian__T *activeSet;
6206 emxArray_real_T_cartesian_way_T *A;
6207 emxArray_real_T_cartesian_way_T *alpha;
6208 emxArray_real_T_cartesian_way_T *AIn;
6209 emxArray_real_T_cartesian_way_T *L;
6210 f_robotics_manip_internal_IKE_T *b;
6211 f_robotics_manip_internal_IKE_T *c;
6212 f_robotics_manip_internal_IKE_T *d;
6213 emxArray_int32_T_cartesian_wa_T *cb;
6214 emxArray_int32_T_cartesian_wa_T *db;
6215 emxArray_int32_T_cartesian_wa_T *eb;
6216 emxArray_int32_T_cartesian_wa_T *fb;
6217 emxArray_int32_T_cartesian_wa_T *gb;
6218 f_robotics_manip_internal_IKE_T *args;
6219 emxArray_real_T_cartesian_way_T *a;
6220 emxArray_int32_T_cartesian_wa_T *ii;
6221 emxArray_real_T_cartesian_way_T *y;
6222 emxArray_int32_T_cartesian_wa_T *ii_0;
6223 emxArray_real_T_cartesian_way_T *y_0;
6224 emxArray_boolean_T_cartesian__T *x;
6225 emxArray_real_T_cartesian_way_T *A_0;
6226 emxArray_real_T_cartesian_way_T *A_1;
6227 emxArray_real_T_cartesian_way_T *A_2;
6228 emxArray_real_T_cartesian_way_T *sigma;
6229 emxArray_real_T_cartesian_way_T *tmp;
6230 emxArray_real_T_cartesian_way_T *tmp_0;
6231 emxArray_real_T_cartesian_way_T *grad_0;
6232 emxArray_real_T_cartesian_way_T *sNew;
6233 emxArray_int32_T_cartesian_wa_T *ii_1;
6234 emxArray_int32_T_cartesian_wa_T *ii_2;
6235 emxArray_real_T_cartesian_way_T *grad_1;
6236 emxArray_real_T_cartesian_way_T *A_3;
6237 emxArray_real_T_cartesian_way_T *alpha_0;
6238 emxArray_int32_T_cartesian_wa_T *ii_3;
6239 static const int8_T tmp_1[36] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1,
6240 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 };
6241
6242 int32_T exitg1;
6243 int32_T exitg2;
6244 boolean_T exitg3;
6245 boolean_T guard1 = false;
6246 boolean_T guard2 = false;
6247 for (cartesian_waypoints_planner_B.i_e = 0; cartesian_waypoints_planner_B.i_e <
6248 6; cartesian_waypoints_planner_B.i_e++) {
6249 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_e] =
6250 obj->SeedInternal[cartesian_waypoints_planner_B.i_e];
6251 }
6252
6253 cartesian_waypoi_emxInit_real_T(&unusedU1, 2);
6254 cartesian_waypoi_emxInit_real_T(&grad, 1);
6255 obj->TimeObjInternal.StartTime = ctimefun();
6256 cartesian_IKHelpers_computeCost(cartesian_waypoints_planner_B.x,
6257 obj->ExtraArgs, &cartesian_waypoints_planner_B.cost,
6258 cartesian_waypoints_planner_B.unusedU0, unusedU1, &b);
6259 obj->ExtraArgs = b;
6260 args = obj->ExtraArgs;
6261 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
6262 grad->size[0] = args->GradTemp->size[0];
6263 cartes_emxEnsureCapacity_real_T(grad, cartesian_waypoints_planner_B.b_i_h);
6264 cartesian_waypoints_planner_B.i_e = args->GradTemp->size[0];
6265 for (cartesian_waypoints_planner_B.b_i_h = 0;
6266 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_e;
6267 cartesian_waypoints_planner_B.b_i_h++) {
6268 grad->data[cartesian_waypoints_planner_B.b_i_h] = args->GradTemp->
6269 data[cartesian_waypoints_planner_B.b_i_h];
6270 }
6271
6272 cartesian_waypoints_planner_eye(cartesian_waypoints_planner_B.unusedU0);
6273 memcpy(&cartesian_waypoints_planner_B.H[0],
6274 &cartesian_waypoints_planner_B.unusedU0[0], 36U * sizeof(real_T));
6275 cartesian_way_emxInit_boolean_T(&activeSet, 1);
6276 cartesian_waypoi_emxInit_real_T(&A, 2);
6277 cartesian_waypoi_emxInit_real_T(&alpha, 1);
6278 cartesian_waypo_emxInit_int32_T(&ii, 1);
6279 if (obj->ConstraintsOn) {
6280 cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];
6281 A->size[0] = obj->ConstraintMatrix->size[0];
6282 A->size[1] = obj->ConstraintMatrix->size[1];
6283 cartes_emxEnsureCapacity_real_T(A, cartesian_waypoints_planner_B.b_i_h);
6284 cartesian_waypoints_planner_B.i_e = obj->ConstraintMatrix->size[0] *
6285 obj->ConstraintMatrix->size[1] - 1;
6286 for (cartesian_waypoints_planner_B.b_i_h = 0;
6287 cartesian_waypoints_planner_B.b_i_h <=
6288 cartesian_waypoints_planner_B.i_e; cartesian_waypoints_planner_B.b_i_h
6289 ++) {
6290 A->data[cartesian_waypoints_planner_B.b_i_h] = obj->ConstraintMatrix->
6291 data[cartesian_waypoints_planner_B.b_i_h];
6292 }
6293
6294 cartesian_waypoints_planner_B.m_k = A->size[1] - 1;
6295 cartesian_waypoints_planner_B.inner = A->size[0] - 1;
6296 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
6297 alpha->size[0] = A->size[1];
6298 cartes_emxEnsureCapacity_real_T(alpha, cartesian_waypoints_planner_B.b_i_h);
6299 for (cartesian_waypoints_planner_B.b_i_h = 0;
6300 cartesian_waypoints_planner_B.b_i_h <=
6301 cartesian_waypoints_planner_B.m_k; cartesian_waypoints_planner_B.b_i_h
6302 ++) {
6303 alpha->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;
6304 }
6305
6306 for (cartesian_waypoints_planner_B.nx_j = 0;
6307 cartesian_waypoints_planner_B.nx_j <=
6308 cartesian_waypoints_planner_B.inner; cartesian_waypoints_planner_B.nx_j
6309 ++) {
6310 for (cartesian_waypoints_planner_B.g_idx_0 = 0;
6311 cartesian_waypoints_planner_B.g_idx_0 <=
6312 cartesian_waypoints_planner_B.m_k;
6313 cartesian_waypoints_planner_B.g_idx_0++) {
6314 alpha->data[cartesian_waypoints_planner_B.g_idx_0] += A->
6315 data[cartesian_waypoints_planner_B.g_idx_0 * A->size[0] +
6316 cartesian_waypoints_planner_B.nx_j] *
6317 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.nx_j];
6318 }
6319 }
6320
6321 cartesian_waypoints_planner_B.b_i_h = activeSet->size[0];
6322 activeSet->size[0] = alpha->size[0];
6323 car_emxEnsureCapacity_boolean_T(activeSet,
6324 cartesian_waypoints_planner_B.b_i_h);
6325 cartesian_waypoints_planner_B.i_e = alpha->size[0];
6326 for (cartesian_waypoints_planner_B.b_i_h = 0;
6327 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_e;
6328 cartesian_waypoints_planner_B.b_i_h++) {
6329 activeSet->data[cartesian_waypoints_planner_B.b_i_h] = (alpha->
6330 data[cartesian_waypoints_planner_B.b_i_h] >= obj->ConstraintBound->
6331 data[cartesian_waypoints_planner_B.b_i_h]);
6332 }
6333
6334 cartesian_waypoints_planner_B.nx_j = activeSet->size[0] - 1;
6335 cartesian_waypoints_planner_B.idx = 0;
6336 for (cartesian_waypoints_planner_B.g_idx_0 = 0;
6337 cartesian_waypoints_planner_B.g_idx_0 <=
6338 cartesian_waypoints_planner_B.nx_j;
6339 cartesian_waypoints_planner_B.g_idx_0++) {
6340 if (activeSet->data[cartesian_waypoints_planner_B.g_idx_0]) {
6341 cartesian_waypoints_planner_B.idx++;
6342 }
6343 }
6344
6345 cartesian_waypoints_planner_B.b_i_h = ii->size[0];
6346 ii->size[0] = cartesian_waypoints_planner_B.idx;
6347 carte_emxEnsureCapacity_int32_T(ii, cartesian_waypoints_planner_B.b_i_h);
6348 cartesian_waypoints_planner_B.b_i_h = 0;
6349 for (cartesian_waypoints_planner_B.g_idx_0 = 0;
6350 cartesian_waypoints_planner_B.g_idx_0 <=
6351 cartesian_waypoints_planner_B.nx_j;
6352 cartesian_waypoints_planner_B.g_idx_0++) {
6353 if (activeSet->data[cartesian_waypoints_planner_B.g_idx_0]) {
6354 ii->data[cartesian_waypoints_planner_B.b_i_h] =
6355 cartesian_waypoints_planner_B.g_idx_0 + 1;
6356 cartesian_waypoints_planner_B.b_i_h++;
6357 }
6358 }
6359
6360 cartesian_waypoints_planner_B.i_e = obj->ConstraintMatrix->size[0];
6361 cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];
6362 A->size[0] = cartesian_waypoints_planner_B.i_e;
6363 A->size[1] = ii->size[0];
6364 cartes_emxEnsureCapacity_real_T(A, cartesian_waypoints_planner_B.b_i_h);
6365 cartesian_waypoints_planner_B.n_b = ii->size[0];
6366 for (cartesian_waypoints_planner_B.b_i_h = 0;
6367 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.n_b;
6368 cartesian_waypoints_planner_B.b_i_h++) {
6369 for (cartesian_waypoints_planner_B.idx = 0;
6370 cartesian_waypoints_planner_B.idx < cartesian_waypoints_planner_B.i_e;
6371 cartesian_waypoints_planner_B.idx++) {
6372 A->data[cartesian_waypoints_planner_B.idx + A->size[0] *
6373 cartesian_waypoints_planner_B.b_i_h] = obj->ConstraintMatrix->data
6374 [(ii->data[cartesian_waypoints_planner_B.b_i_h] - 1) *
6375 obj->ConstraintMatrix->size[0] + cartesian_waypoints_planner_B.idx];
6376 }
6377 }
6378 } else {
6379 cartesian_waypoints_planner_B.g_idx_0 = obj->ConstraintBound->size[0];
6380 cartesian_waypoints_planner_B.b_i_h = activeSet->size[0];
6381 activeSet->size[0] = cartesian_waypoints_planner_B.g_idx_0;
6382 car_emxEnsureCapacity_boolean_T(activeSet,
6383 cartesian_waypoints_planner_B.b_i_h);
6384 for (cartesian_waypoints_planner_B.b_i_h = 0;
6385 cartesian_waypoints_planner_B.b_i_h <
6386 cartesian_waypoints_planner_B.g_idx_0;
6387 cartesian_waypoints_planner_B.b_i_h++) {
6388 activeSet->data[cartesian_waypoints_planner_B.b_i_h] = false;
6389 }
6390
6391 A->size[0] = 6;
6392 A->size[1] = 0;
6393 }
6394
6395 cartesian_waypoints_planner_B.j = A->size[1] - 1;
6396 cartesian_waypoi_emxInit_real_T(&AIn, 2);
6397 cartesian_waypoi_emxInit_real_T(&A_0, 2);
6398 cartesian_waypoi_emxInit_real_T(&A_1, 1);
6399 for (cartesian_waypoints_planner_B.nx_j = 0;
6400 cartesian_waypoints_planner_B.nx_j <= cartesian_waypoints_planner_B.j;
6401 cartesian_waypoints_planner_B.nx_j++) {
6402 cartesian_waypoints_planner_B.i_e = A->size[0];
6403 cartesian_waypoints_planner_B.b_i_h = A_0->size[0] * A_0->size[1];
6404 A_0->size[0] = 1;
6405 A_0->size[1] = cartesian_waypoints_planner_B.i_e;
6406 cartes_emxEnsureCapacity_real_T(A_0, cartesian_waypoints_planner_B.b_i_h);
6407 for (cartesian_waypoints_planner_B.b_i_h = 0;
6408 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_e;
6409 cartesian_waypoints_planner_B.b_i_h++) {
6410 A_0->data[cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *
6411 cartesian_waypoints_planner_B.nx_j + cartesian_waypoints_planner_B.b_i_h];
6412 }
6413
6414 cartesian_waypoints_planner_B.i_e = A->size[0];
6415 cartesian_waypoints_planner_B.b_i_h = A_1->size[0];
6416 A_1->size[0] = cartesian_waypoints_planner_B.i_e;
6417 cartes_emxEnsureCapacity_real_T(A_1, cartesian_waypoints_planner_B.b_i_h);
6418 for (cartesian_waypoints_planner_B.b_i_h = 0;
6419 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_e;
6420 cartesian_waypoints_planner_B.b_i_h++) {
6421 A_1->data[cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *
6422 cartesian_waypoints_planner_B.nx_j + cartesian_waypoints_planner_B.b_i_h];
6423 }
6424
6425 cartesian_waypoints_planner_B.A_d = 0.0;
6426 for (cartesian_waypoints_planner_B.b_i_h = 0;
6427 cartesian_waypoints_planner_B.b_i_h < 6;
6428 cartesian_waypoints_planner_B.b_i_h++) {
6429 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] =
6430 0.0;
6431 for (cartesian_waypoints_planner_B.idx = 0;
6432 cartesian_waypoints_planner_B.idx < 6;
6433 cartesian_waypoints_planner_B.idx++) {
6434 cartesian_waypoints_planner_B.s_p = cartesian_waypoints_planner_B.H[6 *
6435 cartesian_waypoints_planner_B.b_i_h +
6436 cartesian_waypoints_planner_B.idx] * A_0->
6437 data[cartesian_waypoints_planner_B.idx] +
6438 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];
6439 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] =
6440 cartesian_waypoints_planner_B.s_p;
6441 }
6442
6443 cartesian_waypoints_planner_B.A_d +=
6444 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] *
6445 A_1->data[cartesian_waypoints_planner_B.b_i_h];
6446 }
6447
6448 cartesian_waypoints_planner_B.s_p = 1.0 / cartesian_waypoints_planner_B.A_d;
6449 for (cartesian_waypoints_planner_B.b_i_h = 0;
6450 cartesian_waypoints_planner_B.b_i_h < 36;
6451 cartesian_waypoints_planner_B.b_i_h++) {
6452 cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.b_i_h] =
6453 cartesian_waypoints_planner_B.s_p *
6454 cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.b_i_h];
6455 }
6456
6457 cartesian_waypoints_planner_B.i_e = A->size[0];
6458 cartesian_waypoints_planner_B.n_b = A->size[0];
6459 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
6460 AIn->size[0] = cartesian_waypoints_planner_B.i_e;
6461 AIn->size[1] = cartesian_waypoints_planner_B.n_b;
6462 cartes_emxEnsureCapacity_real_T(AIn, cartesian_waypoints_planner_B.b_i_h);
6463 for (cartesian_waypoints_planner_B.b_i_h = 0;
6464 cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.n_b;
6465 cartesian_waypoints_planner_B.b_i_h++) {
6466 for (cartesian_waypoints_planner_B.idx = 0;
6467 cartesian_waypoints_planner_B.idx < cartesian_waypoints_planner_B.i_e;
6468 cartesian_waypoints_planner_B.idx++) {
6469 AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *
6470 cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *
6471 cartesian_waypoints_planner_B.nx_j + cartesian_waypoints_planner_B.idx]
6472 * A->data[A->size[0] * cartesian_waypoints_planner_B.nx_j +
6473 cartesian_waypoints_planner_B.b_i_h];
6474 }
6475 }
6476
6477 cartesian_waypoints_planner_B.n_b = AIn->size[1] - 1;
6478 cartesian_waypoints_planner_B.b_i_h = unusedU1->size[0] * unusedU1->size[1];
6479 unusedU1->size[0] = 6;
6480 unusedU1->size[1] = AIn->size[1];
6481 cartes_emxEnsureCapacity_real_T(unusedU1,
6482 cartesian_waypoints_planner_B.b_i_h);
6483 for (cartesian_waypoints_planner_B.idx = 0;
6484 cartesian_waypoints_planner_B.idx <= cartesian_waypoints_planner_B.n_b;
6485 cartesian_waypoints_planner_B.idx++) {
6486 cartesian_waypoints_planner_B.coffset = cartesian_waypoints_planner_B.idx *
6487 6 - 1;
6488 cartesian_waypoints_planner_B.boffset = cartesian_waypoints_planner_B.idx *
6489 AIn->size[0] - 1;
6490 for (cartesian_waypoints_planner_B.b_i_h = 0;
6491 cartesian_waypoints_planner_B.b_i_h < 6;
6492 cartesian_waypoints_planner_B.b_i_h++) {
6493 cartesian_waypoints_planner_B.s_p = 0.0;
6494 for (cartesian_waypoints_planner_B.g_idx_0 = 0;
6495 cartesian_waypoints_planner_B.g_idx_0 < 6;
6496 cartesian_waypoints_planner_B.g_idx_0++) {
6497 cartesian_waypoints_planner_B.s_p +=
6498 cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.g_idx_0
6499 * 6 + cartesian_waypoints_planner_B.b_i_h] * AIn->data
6500 [(cartesian_waypoints_planner_B.boffset +
6501 cartesian_waypoints_planner_B.g_idx_0) + 1];
6502 }
6503
6504 unusedU1->data[(cartesian_waypoints_planner_B.coffset +
6505 cartesian_waypoints_planner_B.b_i_h) + 1] =
6506 cartesian_waypoints_planner_B.s_p;
6507 }
6508 }
6509
6510 for (cartesian_waypoints_planner_B.b_i_h = 0;
6511 cartesian_waypoints_planner_B.b_i_h < 6;
6512 cartesian_waypoints_planner_B.b_i_h++) {
6513 for (cartesian_waypoints_planner_B.idx = 0;
6514 cartesian_waypoints_planner_B.idx < 6;
6515 cartesian_waypoints_planner_B.idx++) {
6516 cartesian_waypoints_planner_B.s_p = 0.0;
6517 for (cartesian_waypoints_planner_B.i_e = 0;
6518 cartesian_waypoints_planner_B.i_e < 6;
6519 cartesian_waypoints_planner_B.i_e++) {
6520 cartesian_waypoints_planner_B.s_p += unusedU1->data[6 *
6521 cartesian_waypoints_planner_B.i_e +
6522 cartesian_waypoints_planner_B.b_i_h] *
6523 cartesian_waypoints_planner_B.H[6 *
6524 cartesian_waypoints_planner_B.idx +
6525 cartesian_waypoints_planner_B.i_e];
6526 }
6527
6528 cartesian_waypoints_planner_B.idxl = 6 *
6529 cartesian_waypoints_planner_B.idx +
6530 cartesian_waypoints_planner_B.b_i_h;
6531 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.idxl] =
6532 cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.idxl] -
6533 cartesian_waypoints_planner_B.s_p;
6534 }
6535 }
6536
6537 memcpy(&cartesian_waypoints_planner_B.H[0],
6538 &cartesian_waypoints_planner_B.H_m[0], 36U * sizeof(real_T));
6539 }
6540
6541 cartesian_waypoi_emxFree_real_T(&A_1);
6542 cartesian_waypoi_emxFree_real_T(&A_0);
6543 for (cartesian_waypoints_planner_B.i_e = 0; cartesian_waypoints_planner_B.i_e <
6544 6; cartesian_waypoints_planner_B.i_e++) {
6545 xSol[cartesian_waypoints_planner_B.i_e] =
6546 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_e];
6547 }
6548
6549 cartesian_waypoints_planner_B.A_d = obj->MaxNumIterationInternal;
6550 cartesian_waypoints_planner_B.g_idx_0 = 0;
6551 cartesian_waypoi_emxInit_real_T(&L, 1);
6552 cartesian_waypo_emxInit_int32_T(&cb, 1);
6553 cartesian_waypo_emxInit_int32_T(&db, 1);
6554 cartesian_waypo_emxInit_int32_T(&eb, 1);
6555 cartesian_waypo_emxInit_int32_T(&fb, 1);
6556 cartesian_waypo_emxInit_int32_T(&gb, 1);
6557 cartesian_waypoi_emxInit_real_T(&a, 2);
6558 cartesian_waypoi_emxInit_real_T(&y, 1);
6559 cartesian_waypo_emxInit_int32_T(&ii_0, 1);
6560 cartesian_waypoi_emxInit_real_T(&y_0, 2);
6561 cartesian_way_emxInit_boolean_T(&x, 1);
6562 cartesian_waypoi_emxInit_real_T(&A_2, 2);
6563 cartesian_waypoi_emxInit_real_T(&sigma, 2);
6564 cartesian_waypoi_emxInit_real_T(&tmp, 2);
6565 cartesian_waypoi_emxInit_real_T(&tmp_0, 2);
6566 cartesian_waypoi_emxInit_real_T(&grad_0, 2);
6567 cartesian_waypoi_emxInit_real_T(&sNew, 2);
6568 cartesian_waypo_emxInit_int32_T(&ii_1, 1);
6569 cartesian_waypo_emxInit_int32_T(&ii_2, 1);
6570 cartesian_waypoi_emxInit_real_T(&grad_1, 2);
6571 cartesian_waypoi_emxInit_real_T(&A_3, 2);
6572 cartesian_waypoi_emxInit_real_T(&alpha_0, 2);
6573 cartesian_waypo_emxInit_int32_T(&ii_3, 1);
6574 do {
6575 exitg2 = 0;
6576 if (cartesian_waypoints_planner_B.g_idx_0 <= static_cast<int32_T>
6577 (cartesian_waypoints_planner_B.A_d) - 1) {
6578 cartesian_waypoints_planner_B.s_p = SystemTimeProvider_getElapsedTi
6579 (&obj->TimeObjInternal);
6580 cartesian_waypoints_planner_B.flag = (cartesian_waypoints_planner_B.s_p >
6581 obj->MaxTimeInternal);
6582 if (cartesian_waypoints_planner_B.flag) {
6583 *exitFlag = TimeLimitExceeded;
6584 args = obj->ExtraArgs;
6585 for (cartesian_waypoints_planner_B.b_i_h = 0;
6586 cartesian_waypoints_planner_B.b_i_h < 36;
6587 cartesian_waypoints_planner_B.b_i_h++) {
6588 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
6589 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
6590 }
6591
6592 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
6593 grad->size[0] = args->ErrTemp->size[0];
6594 cartes_emxEnsureCapacity_real_T(grad,
6595 cartesian_waypoints_planner_B.b_i_h);
6596 cartesian_waypoints_planner_B.i_e = args->ErrTemp->size[0];
6597 for (cartesian_waypoints_planner_B.b_i_h = 0;
6598 cartesian_waypoints_planner_B.b_i_h <
6599 cartesian_waypoints_planner_B.i_e;
6600 cartesian_waypoints_planner_B.b_i_h++) {
6601 grad->data[cartesian_waypoints_planner_B.b_i_h] = args->ErrTemp->
6602 data[cartesian_waypoints_planner_B.b_i_h];
6603 }
6604
6605 for (cartesian_waypoints_planner_B.b_i_h = 0;
6606 cartesian_waypoints_planner_B.b_i_h < 6;
6607 cartesian_waypoints_planner_B.b_i_h++) {
6608 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h] =
6609 0.0;
6610 for (cartesian_waypoints_planner_B.idx = 0;
6611 cartesian_waypoints_planner_B.idx < 6;
6612 cartesian_waypoints_planner_B.idx++) {
6613 cartesian_waypoints_planner_B.A_d =
6614 cartesian_waypoints_planner_B.unusedU0[6 *
6615 cartesian_waypoints_planner_B.idx +
6616 cartesian_waypoints_planner_B.b_i_h] * grad->
6617 data[cartesian_waypoints_planner_B.idx] +
6618 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
6619 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
6620 = cartesian_waypoints_planner_B.A_d;
6621 }
6622 }
6623
6624 *err = cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.x);
6625 *iter = static_cast<real_T>(cartesian_waypoints_planner_B.g_idx_0) + 1.0;
6626 exitg2 = 1;
6627 } else {
6628 if ((A->size[0] == 0) || (A->size[1] == 0)) {
6629 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
6630 alpha->size[0] = 1;
6631 cartes_emxEnsureCapacity_real_T(alpha,
6632 cartesian_waypoints_planner_B.b_i_h);
6633 alpha->data[0] = 0.0;
6634 } else {
6635 cartesian_waypoints_planner_B.m_k = A->size[1] - 1;
6636 cartesian_waypoints_planner_B.inner = A->size[0] - 1;
6637 cartesian_waypoints_planner_B.n_b = A->size[1] - 1;
6638 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
6639 AIn->size[0] = A->size[1];
6640 AIn->size[1] = A->size[1];
6641 cartes_emxEnsureCapacity_real_T(AIn,
6642 cartesian_waypoints_planner_B.b_i_h);
6643 for (cartesian_waypoints_planner_B.idx = 0;
6644 cartesian_waypoints_planner_B.idx <=
6645 cartesian_waypoints_planner_B.n_b;
6646 cartesian_waypoints_planner_B.idx++) {
6647 cartesian_waypoints_planner_B.coffset =
6648 (cartesian_waypoints_planner_B.m_k + 1) *
6649 cartesian_waypoints_planner_B.idx - 1;
6650 cartesian_waypoints_planner_B.boffset =
6651 cartesian_waypoints_planner_B.idx * A->size[0] - 1;
6652 for (cartesian_waypoints_planner_B.b_i_h = 0;
6653 cartesian_waypoints_planner_B.b_i_h <=
6654 cartesian_waypoints_planner_B.m_k;
6655 cartesian_waypoints_planner_B.b_i_h++) {
6656 AIn->data[(cartesian_waypoints_planner_B.coffset +
6657 cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;
6658 }
6659
6660 for (cartesian_waypoints_planner_B.nx_j = 0;
6661 cartesian_waypoints_planner_B.nx_j <=
6662 cartesian_waypoints_planner_B.inner;
6663 cartesian_waypoints_planner_B.nx_j++) {
6664 cartesian_waypoints_planner_B.s_p = A->data
6665 [(cartesian_waypoints_planner_B.boffset +
6666 cartesian_waypoints_planner_B.nx_j) + 1];
6667 for (cartesian_waypoints_planner_B.j = 0;
6668 cartesian_waypoints_planner_B.j <=
6669 cartesian_waypoints_planner_B.m_k;
6670 cartesian_waypoints_planner_B.j++) {
6671 cartesian_waypoints_planner_B.b_i_h =
6672 (cartesian_waypoints_planner_B.coffset +
6673 cartesian_waypoints_planner_B.j) + 1;
6674 AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->
6675 data[cartesian_waypoints_planner_B.j * A->size[0] +
6676 cartesian_waypoints_planner_B.nx_j] *
6677 cartesian_waypoints_planner_B.s_p;
6678 }
6679 }
6680 }
6681
6682 cartesian_waypoints_planner_B.b_i_h = A_2->size[0] * A_2->size[1];
6683 A_2->size[0] = A->size[1];
6684 A_2->size[1] = A->size[0];
6685 cartes_emxEnsureCapacity_real_T(A_2,
6686 cartesian_waypoints_planner_B.b_i_h);
6687 cartesian_waypoints_planner_B.i_e = A->size[0];
6688 for (cartesian_waypoints_planner_B.b_i_h = 0;
6689 cartesian_waypoints_planner_B.b_i_h <
6690 cartesian_waypoints_planner_B.i_e;
6691 cartesian_waypoints_planner_B.b_i_h++) {
6692 cartesian_waypoints_planner_B.n_b = A->size[1];
6693 for (cartesian_waypoints_planner_B.idx = 0;
6694 cartesian_waypoints_planner_B.idx <
6695 cartesian_waypoints_planner_B.n_b;
6696 cartesian_waypoints_planner_B.idx++) {
6697 A_2->data[cartesian_waypoints_planner_B.idx + A_2->size[0] *
6698 cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *
6699 cartesian_waypoints_planner_B.idx +
6700 cartesian_waypoints_planner_B.b_i_h];
6701 }
6702 }
6703
6704 cartesian_waypoints_pl_mldivide(AIn, A_2, a);
6705 cartesian_waypoints_planner_B.m_k = a->size[0] - 1;
6706 cartesian_waypoints_planner_B.inner = a->size[1] - 1;
6707 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
6708 alpha->size[0] = a->size[0];
6709 cartes_emxEnsureCapacity_real_T(alpha,
6710 cartesian_waypoints_planner_B.b_i_h);
6711 for (cartesian_waypoints_planner_B.b_i_h = 0;
6712 cartesian_waypoints_planner_B.b_i_h <=
6713 cartesian_waypoints_planner_B.m_k;
6714 cartesian_waypoints_planner_B.b_i_h++) {
6715 alpha->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;
6716 }
6717
6718 for (cartesian_waypoints_planner_B.nx_j = 0;
6719 cartesian_waypoints_planner_B.nx_j <=
6720 cartesian_waypoints_planner_B.inner;
6721 cartesian_waypoints_planner_B.nx_j++) {
6722 cartesian_waypoints_planner_B.aoffset =
6723 cartesian_waypoints_planner_B.nx_j * a->size[0] - 1;
6724 for (cartesian_waypoints_planner_B.j = 0;
6725 cartesian_waypoints_planner_B.j <=
6726 cartesian_waypoints_planner_B.m_k;
6727 cartesian_waypoints_planner_B.j++) {
6728 alpha->data[cartesian_waypoints_planner_B.j] += a->data
6729 [(cartesian_waypoints_planner_B.aoffset +
6730 cartesian_waypoints_planner_B.j) + 1] * grad->
6731 data[cartesian_waypoints_planner_B.nx_j];
6732 }
6733 }
6734 }
6735
6736 for (cartesian_waypoints_planner_B.b_i_h = 0;
6737 cartesian_waypoints_planner_B.b_i_h < 6;
6738 cartesian_waypoints_planner_B.b_i_h++) {
6739 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] =
6740 0.0;
6741 for (cartesian_waypoints_planner_B.idx = 0;
6742 cartesian_waypoints_planner_B.idx < 6;
6743 cartesian_waypoints_planner_B.idx++) {
6744 cartesian_waypoints_planner_B.b_gamma =
6745 cartesian_waypoints_planner_B.H[6 *
6746 cartesian_waypoints_planner_B.idx +
6747 cartesian_waypoints_planner_B.b_i_h] * grad->
6748 data[cartesian_waypoints_planner_B.idx] +
6749 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];
6750 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
6751 = cartesian_waypoints_planner_B.b_gamma;
6752 }
6753 }
6754
6755 if (DampedBFGSwGradientProjection_a(obj,
6756 cartesian_waypoints_planner_B.Hg, alpha)) {
6757 *exitFlag = LocalMinimumFound;
6758 args = obj->ExtraArgs;
6759 for (cartesian_waypoints_planner_B.b_i_h = 0;
6760 cartesian_waypoints_planner_B.b_i_h < 36;
6761 cartesian_waypoints_planner_B.b_i_h++) {
6762 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
6763 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
6764 }
6765
6766 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
6767 grad->size[0] = args->ErrTemp->size[0];
6768 cartes_emxEnsureCapacity_real_T(grad,
6769 cartesian_waypoints_planner_B.b_i_h);
6770 cartesian_waypoints_planner_B.i_e = args->ErrTemp->size[0];
6771 for (cartesian_waypoints_planner_B.b_i_h = 0;
6772 cartesian_waypoints_planner_B.b_i_h <
6773 cartesian_waypoints_planner_B.i_e;
6774 cartesian_waypoints_planner_B.b_i_h++) {
6775 grad->data[cartesian_waypoints_planner_B.b_i_h] = args->
6776 ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];
6777 }
6778
6779 for (cartesian_waypoints_planner_B.b_i_h = 0;
6780 cartesian_waypoints_planner_B.b_i_h < 6;
6781 cartesian_waypoints_planner_B.b_i_h++) {
6782 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
6783 = 0.0;
6784 for (cartesian_waypoints_planner_B.idx = 0;
6785 cartesian_waypoints_planner_B.idx < 6;
6786 cartesian_waypoints_planner_B.idx++) {
6787 cartesian_waypoints_planner_B.A_d =
6788 cartesian_waypoints_planner_B.unusedU0[6 *
6789 cartesian_waypoints_planner_B.idx +
6790 cartesian_waypoints_planner_B.b_i_h] * grad->
6791 data[cartesian_waypoints_planner_B.idx] +
6792 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
6793 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
6794 = cartesian_waypoints_planner_B.A_d;
6795 }
6796 }
6797
6798 *err = cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.x);
6799 *iter = static_cast<real_T>(cartesian_waypoints_planner_B.g_idx_0) +
6800 1.0;
6801 exitg2 = 1;
6802 } else {
6803 guard1 = false;
6804 guard2 = false;
6805 if (obj->ConstraintsOn && ((A->size[0] != 0) && (A->size[1] != 0))) {
6806 cartesian_waypoints_planner_B.m_k = A->size[1] - 1;
6807 cartesian_waypoints_planner_B.inner = A->size[0] - 1;
6808 cartesian_waypoints_planner_B.n_b = A->size[1] - 1;
6809 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
6810 AIn->size[0] = A->size[1];
6811 AIn->size[1] = A->size[1];
6812 cartes_emxEnsureCapacity_real_T(AIn,
6813 cartesian_waypoints_planner_B.b_i_h);
6814 for (cartesian_waypoints_planner_B.idx = 0;
6815 cartesian_waypoints_planner_B.idx <=
6816 cartesian_waypoints_planner_B.n_b;
6817 cartesian_waypoints_planner_B.idx++) {
6818 cartesian_waypoints_planner_B.coffset =
6819 (cartesian_waypoints_planner_B.m_k + 1) *
6820 cartesian_waypoints_planner_B.idx - 1;
6821 cartesian_waypoints_planner_B.boffset =
6822 cartesian_waypoints_planner_B.idx * A->size[0] - 1;
6823 for (cartesian_waypoints_planner_B.b_i_h = 0;
6824 cartesian_waypoints_planner_B.b_i_h <=
6825 cartesian_waypoints_planner_B.m_k;
6826 cartesian_waypoints_planner_B.b_i_h++) {
6827 AIn->data[(cartesian_waypoints_planner_B.coffset +
6828 cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;
6829 }
6830
6831 for (cartesian_waypoints_planner_B.nx_j = 0;
6832 cartesian_waypoints_planner_B.nx_j <=
6833 cartesian_waypoints_planner_B.inner;
6834 cartesian_waypoints_planner_B.nx_j++) {
6835 cartesian_waypoints_planner_B.s_p = A->data
6836 [(cartesian_waypoints_planner_B.boffset +
6837 cartesian_waypoints_planner_B.nx_j) + 1];
6838 for (cartesian_waypoints_planner_B.j = 0;
6839 cartesian_waypoints_planner_B.j <=
6840 cartesian_waypoints_planner_B.m_k;
6841 cartesian_waypoints_planner_B.j++) {
6842 cartesian_waypoints_planner_B.b_i_h =
6843 (cartesian_waypoints_planner_B.coffset +
6844 cartesian_waypoints_planner_B.j) + 1;
6845 AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->
6846 data[cartesian_waypoints_planner_B.j * A->size[0] +
6847 cartesian_waypoints_planner_B.nx_j] *
6848 cartesian_waypoints_planner_B.s_p;
6849 }
6850 }
6851 }
6852
6853 cartesian_waypoints_planner_inv(AIn, a);
6854 cartesian_waypoints_planne_diag(a, L);
6855 cartesian_waypoints_pla_sqrt_ev(L);
6856 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
6857 cartes_emxEnsureCapacity_real_T(alpha,
6858 cartesian_waypoints_planner_B.b_i_h);
6859 cartesian_waypoints_planner_B.i_e = alpha->size[0];
6860 for (cartesian_waypoints_planner_B.b_i_h = 0;
6861 cartesian_waypoints_planner_B.b_i_h <
6862 cartesian_waypoints_planner_B.i_e;
6863 cartesian_waypoints_planner_B.b_i_h++) {
6864 alpha->data[cartesian_waypoints_planner_B.b_i_h] /= L->
6865 data[cartesian_waypoints_planner_B.b_i_h];
6866 }
6867
6868 cartesian_waypoints_planner_B.n_b = alpha->size[0];
6869 if (alpha->size[0] <= 2) {
6870 if (alpha->size[0] == 1) {
6871 cartesian_waypoints_planner_B.s_p = alpha->data[0];
6872 cartesian_waypoints_planner_B.idxl = 0;
6873 } else if ((alpha->data[0] < alpha->data[1]) || (rtIsNaN
6874 (alpha->data[0]) && (!rtIsNaN(alpha->data[1])))) {
6875 cartesian_waypoints_planner_B.s_p = alpha->data[1];
6876 cartesian_waypoints_planner_B.idxl = 1;
6877 } else {
6878 cartesian_waypoints_planner_B.s_p = alpha->data[0];
6879 cartesian_waypoints_planner_B.idxl = 0;
6880 }
6881 } else {
6882 if (!rtIsNaN(alpha->data[0])) {
6883 cartesian_waypoints_planner_B.idxl = 1;
6884 } else {
6885 cartesian_waypoints_planner_B.idxl = 0;
6886 cartesian_waypoints_planner_B.b_i_h = 2;
6887 exitg3 = false;
6888 while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h <=
6889 alpha->size[0])) {
6890 if (!rtIsNaN(alpha->data[cartesian_waypoints_planner_B.b_i_h -
6891 1])) {
6892 cartesian_waypoints_planner_B.idxl =
6893 cartesian_waypoints_planner_B.b_i_h;
6894 exitg3 = true;
6895 } else {
6896 cartesian_waypoints_planner_B.b_i_h++;
6897 }
6898 }
6899 }
6900
6901 if (cartesian_waypoints_planner_B.idxl == 0) {
6902 cartesian_waypoints_planner_B.s_p = alpha->data[0];
6903 } else {
6904 cartesian_waypoints_planner_B.s_p = alpha->
6905 data[cartesian_waypoints_planner_B.idxl - 1];
6906 cartesian_waypoints_planner_B.nx_j =
6907 cartesian_waypoints_planner_B.idxl;
6908 for (cartesian_waypoints_planner_B.b_i_h =
6909 cartesian_waypoints_planner_B.idxl + 1;
6910 cartesian_waypoints_planner_B.b_i_h <=
6911 cartesian_waypoints_planner_B.n_b;
6912 cartesian_waypoints_planner_B.b_i_h++) {
6913 if (cartesian_waypoints_planner_B.s_p < alpha->
6914 data[cartesian_waypoints_planner_B.b_i_h - 1]) {
6915 cartesian_waypoints_planner_B.s_p = alpha->
6916 data[cartesian_waypoints_planner_B.b_i_h - 1];
6917 cartesian_waypoints_planner_B.nx_j =
6918 cartesian_waypoints_planner_B.b_i_h;
6919 }
6920 }
6921
6922 cartesian_waypoints_planner_B.idxl =
6923 cartesian_waypoints_planner_B.nx_j - 1;
6924 }
6925 }
6926
6927 if (cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.Hg)
6928 < 0.5 * cartesian_waypoints_planner_B.s_p) {
6929 cartesian_waypoints_planner_B.nx_j = activeSet->size[0];
6930 cartesian_waypoints_planner_B.idx = 0;
6931 cartesian_waypoints_planner_B.b_i_h = ii->size[0];
6932 ii->size[0] = activeSet->size[0];
6933 carte_emxEnsureCapacity_int32_T(ii,
6934 cartesian_waypoints_planner_B.b_i_h);
6935 cartesian_waypoints_planner_B.b_i_h = 1;
6936 exitg3 = false;
6937 while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h - 1 <=
6938 cartesian_waypoints_planner_B.nx_j - 1)) {
6939 if (activeSet->data[cartesian_waypoints_planner_B.b_i_h - 1]) {
6940 cartesian_waypoints_planner_B.idx++;
6941 ii->data[cartesian_waypoints_planner_B.idx - 1] =
6942 cartesian_waypoints_planner_B.b_i_h;
6943 if (cartesian_waypoints_planner_B.idx >=
6944 cartesian_waypoints_planner_B.nx_j) {
6945 exitg3 = true;
6946 } else {
6947 cartesian_waypoints_planner_B.b_i_h++;
6948 }
6949 } else {
6950 cartesian_waypoints_planner_B.b_i_h++;
6951 }
6952 }
6953
6954 if (activeSet->size[0] == 1) {
6955 if (cartesian_waypoints_planner_B.idx == 0) {
6956 ii->size[0] = 0;
6957 }
6958 } else {
6959 if (1 > cartesian_waypoints_planner_B.idx) {
6960 cartesian_waypoints_planner_B.idx = 0;
6961 }
6962
6963 cartesian_waypoints_planner_B.b_i_h = ii_1->size[0];
6964 ii_1->size[0] = cartesian_waypoints_planner_B.idx;
6965 carte_emxEnsureCapacity_int32_T(ii_1,
6966 cartesian_waypoints_planner_B.b_i_h);
6967 for (cartesian_waypoints_planner_B.b_i_h = 0;
6968 cartesian_waypoints_planner_B.b_i_h <
6969 cartesian_waypoints_planner_B.idx;
6970 cartesian_waypoints_planner_B.b_i_h++) {
6971 ii_1->data[cartesian_waypoints_planner_B.b_i_h] = ii->
6972 data[cartesian_waypoints_planner_B.b_i_h];
6973 }
6974
6975 cartesian_waypoints_planner_B.b_i_h = ii->size[0];
6976 ii->size[0] = ii_1->size[0];
6977 carte_emxEnsureCapacity_int32_T(ii,
6978 cartesian_waypoints_planner_B.b_i_h);
6979 cartesian_waypoints_planner_B.i_e = ii_1->size[0];
6980 for (cartesian_waypoints_planner_B.b_i_h = 0;
6981 cartesian_waypoints_planner_B.b_i_h <
6982 cartesian_waypoints_planner_B.i_e;
6983 cartesian_waypoints_planner_B.b_i_h++) {
6984 ii->data[cartesian_waypoints_planner_B.b_i_h] = ii_1->
6985 data[cartesian_waypoints_planner_B.b_i_h];
6986 }
6987 }
6988
6989 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
6990 alpha->size[0] = ii->size[0];
6991 cartes_emxEnsureCapacity_real_T(alpha,
6992 cartesian_waypoints_planner_B.b_i_h);
6993 cartesian_waypoints_planner_B.i_e = ii->size[0];
6994 for (cartesian_waypoints_planner_B.b_i_h = 0;
6995 cartesian_waypoints_planner_B.b_i_h <
6996 cartesian_waypoints_planner_B.i_e;
6997 cartesian_waypoints_planner_B.b_i_h++) {
6998 alpha->data[cartesian_waypoints_planner_B.b_i_h] = ii->
6999 data[cartesian_waypoints_planner_B.b_i_h];
7000 }
7001
7002 activeSet->data[static_cast<int32_T>(alpha->
7003 data[cartesian_waypoints_planner_B.idxl]) - 1] = false;
7004 cartesian_waypoints_planner_B.nx_j = activeSet->size[0] - 1;
7005 cartesian_waypoints_planner_B.idx = 0;
7006 for (cartesian_waypoints_planner_B.b_i_h = 0;
7007 cartesian_waypoints_planner_B.b_i_h <=
7008 cartesian_waypoints_planner_B.nx_j;
7009 cartesian_waypoints_planner_B.b_i_h++) {
7010 if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7011 cartesian_waypoints_planner_B.idx++;
7012 }
7013 }
7014
7015 cartesian_waypoints_planner_B.b_i_h = eb->size[0];
7016 eb->size[0] = cartesian_waypoints_planner_B.idx;
7017 carte_emxEnsureCapacity_int32_T(eb,
7018 cartesian_waypoints_planner_B.b_i_h);
7019 cartesian_waypoints_planner_B.idx = 0;
7020 for (cartesian_waypoints_planner_B.b_i_h = 0;
7021 cartesian_waypoints_planner_B.b_i_h <=
7022 cartesian_waypoints_planner_B.nx_j;
7023 cartesian_waypoints_planner_B.b_i_h++) {
7024 if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7025 eb->data[cartesian_waypoints_planner_B.idx] =
7026 cartesian_waypoints_planner_B.b_i_h + 1;
7027 cartesian_waypoints_planner_B.idx++;
7028 }
7029 }
7030
7031 cartesian_waypoints_planner_B.i_e = obj->ConstraintMatrix->size[0];
7032 cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];
7033 A->size[0] = cartesian_waypoints_planner_B.i_e;
7034 A->size[1] = eb->size[0];
7035 cartes_emxEnsureCapacity_real_T(A,
7036 cartesian_waypoints_planner_B.b_i_h);
7037 cartesian_waypoints_planner_B.n_b = eb->size[0];
7038 for (cartesian_waypoints_planner_B.b_i_h = 0;
7039 cartesian_waypoints_planner_B.b_i_h <
7040 cartesian_waypoints_planner_B.n_b;
7041 cartesian_waypoints_planner_B.b_i_h++) {
7042 for (cartesian_waypoints_planner_B.idx = 0;
7043 cartesian_waypoints_planner_B.idx <
7044 cartesian_waypoints_planner_B.i_e;
7045 cartesian_waypoints_planner_B.idx++) {
7046 A->data[cartesian_waypoints_planner_B.idx + A->size[0] *
7047 cartesian_waypoints_planner_B.b_i_h] = obj->
7048 ConstraintMatrix->data[(eb->
7049 data[cartesian_waypoints_planner_B.b_i_h] - 1) *
7050 obj->ConstraintMatrix->size[0] +
7051 cartesian_waypoints_planner_B.idx];
7052 }
7053 }
7054
7055 cartesian_waypoints_planner_B.m_k = A->size[1] - 1;
7056 cartesian_waypoints_planner_B.inner = A->size[0] - 1;
7057 cartesian_waypoints_planner_B.n_b = A->size[1] - 1;
7058 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
7059 AIn->size[0] = A->size[1];
7060 AIn->size[1] = A->size[1];
7061 cartes_emxEnsureCapacity_real_T(AIn,
7062 cartesian_waypoints_planner_B.b_i_h);
7063 for (cartesian_waypoints_planner_B.idx = 0;
7064 cartesian_waypoints_planner_B.idx <=
7065 cartesian_waypoints_planner_B.n_b;
7066 cartesian_waypoints_planner_B.idx++) {
7067 cartesian_waypoints_planner_B.coffset =
7068 (cartesian_waypoints_planner_B.m_k + 1) *
7069 cartesian_waypoints_planner_B.idx - 1;
7070 cartesian_waypoints_planner_B.boffset =
7071 cartesian_waypoints_planner_B.idx * A->size[0] - 1;
7072 for (cartesian_waypoints_planner_B.b_i_h = 0;
7073 cartesian_waypoints_planner_B.b_i_h <=
7074 cartesian_waypoints_planner_B.m_k;
7075 cartesian_waypoints_planner_B.b_i_h++) {
7076 AIn->data[(cartesian_waypoints_planner_B.coffset +
7077 cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;
7078 }
7079
7080 for (cartesian_waypoints_planner_B.nx_j = 0;
7081 cartesian_waypoints_planner_B.nx_j <=
7082 cartesian_waypoints_planner_B.inner;
7083 cartesian_waypoints_planner_B.nx_j++) {
7084 cartesian_waypoints_planner_B.s_p = A->data
7085 [(cartesian_waypoints_planner_B.boffset +
7086 cartesian_waypoints_planner_B.nx_j) + 1];
7087 for (cartesian_waypoints_planner_B.j = 0;
7088 cartesian_waypoints_planner_B.j <=
7089 cartesian_waypoints_planner_B.m_k;
7090 cartesian_waypoints_planner_B.j++) {
7091 cartesian_waypoints_planner_B.b_i_h =
7092 (cartesian_waypoints_planner_B.coffset +
7093 cartesian_waypoints_planner_B.j) + 1;
7094 AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->
7095 data[cartesian_waypoints_planner_B.j * A->size[0] +
7096 cartesian_waypoints_planner_B.nx_j] *
7097 cartesian_waypoints_planner_B.s_p;
7098 }
7099 }
7100 }
7101
7102 cartesian_waypoints_planner_B.b_i_h = A_3->size[0] * A_3->size[1];
7103 A_3->size[0] = A->size[1];
7104 A_3->size[1] = A->size[0];
7105 cartes_emxEnsureCapacity_real_T(A_3,
7106 cartesian_waypoints_planner_B.b_i_h);
7107 cartesian_waypoints_planner_B.i_e = A->size[0];
7108 for (cartesian_waypoints_planner_B.b_i_h = 0;
7109 cartesian_waypoints_planner_B.b_i_h <
7110 cartesian_waypoints_planner_B.i_e;
7111 cartesian_waypoints_planner_B.b_i_h++) {
7112 cartesian_waypoints_planner_B.n_b = A->size[1];
7113 for (cartesian_waypoints_planner_B.idx = 0;
7114 cartesian_waypoints_planner_B.idx <
7115 cartesian_waypoints_planner_B.n_b;
7116 cartesian_waypoints_planner_B.idx++) {
7117 A_3->data[cartesian_waypoints_planner_B.idx + A_3->size[0] *
7118 cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *
7119 cartesian_waypoints_planner_B.idx +
7120 cartesian_waypoints_planner_B.b_i_h];
7121 }
7122 }
7123
7124 cartesian_waypoints_pl_mldivide(AIn, A_3, a);
7125 cartesian_waypoints_planner_B.m_k = A->size[0] - 1;
7126 cartesian_waypoints_planner_B.inner = A->size[1] - 1;
7127 cartesian_waypoints_planner_B.n_b = a->size[1] - 1;
7128 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
7129 AIn->size[0] = A->size[0];
7130 AIn->size[1] = a->size[1];
7131 cartes_emxEnsureCapacity_real_T(AIn,
7132 cartesian_waypoints_planner_B.b_i_h);
7133 for (cartesian_waypoints_planner_B.idx = 0;
7134 cartesian_waypoints_planner_B.idx <=
7135 cartesian_waypoints_planner_B.n_b;
7136 cartesian_waypoints_planner_B.idx++) {
7137 cartesian_waypoints_planner_B.coffset =
7138 (cartesian_waypoints_planner_B.m_k + 1) *
7139 cartesian_waypoints_planner_B.idx - 1;
7140 cartesian_waypoints_planner_B.boffset =
7141 cartesian_waypoints_planner_B.idx * a->size[0] - 1;
7142 for (cartesian_waypoints_planner_B.b_i_h = 0;
7143 cartesian_waypoints_planner_B.b_i_h <=
7144 cartesian_waypoints_planner_B.m_k;
7145 cartesian_waypoints_planner_B.b_i_h++) {
7146 AIn->data[(cartesian_waypoints_planner_B.coffset +
7147 cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;
7148 }
7149
7150 for (cartesian_waypoints_planner_B.nx_j = 0;
7151 cartesian_waypoints_planner_B.nx_j <=
7152 cartesian_waypoints_planner_B.inner;
7153 cartesian_waypoints_planner_B.nx_j++) {
7154 cartesian_waypoints_planner_B.aoffset =
7155 cartesian_waypoints_planner_B.nx_j * A->size[0] - 1;
7156 cartesian_waypoints_planner_B.s_p = a->data
7157 [(cartesian_waypoints_planner_B.boffset +
7158 cartesian_waypoints_planner_B.nx_j) + 1];
7159 for (cartesian_waypoints_planner_B.j = 0;
7160 cartesian_waypoints_planner_B.j <=
7161 cartesian_waypoints_planner_B.m_k;
7162 cartesian_waypoints_planner_B.j++) {
7163 cartesian_waypoints_planner_B.i_e =
7164 cartesian_waypoints_planner_B.j + 1;
7165 cartesian_waypoints_planner_B.b_i_h =
7166 cartesian_waypoints_planner_B.coffset +
7167 cartesian_waypoints_planner_B.i_e;
7168 AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->
7169 data[cartesian_waypoints_planner_B.aoffset +
7170 cartesian_waypoints_planner_B.i_e] *
7171 cartesian_waypoints_planner_B.s_p;
7172 }
7173 }
7174 }
7175
7176 for (cartesian_waypoints_planner_B.b_i_h = 0;
7177 cartesian_waypoints_planner_B.b_i_h < 36;
7178 cartesian_waypoints_planner_B.b_i_h++) {
7179 cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.b_i_h]
7180 =
7181 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
7182 - AIn->data[cartesian_waypoints_planner_B.b_i_h];
7183 }
7184
7185 cartesian_waypoints_planner_B.s_p = alpha->
7186 data[cartesian_waypoints_planner_B.idxl];
7187 cartesian_waypoints_planner_B.b_i_h = static_cast<int32_T>
7188 (cartesian_waypoints_planner_B.s_p);
7189 cartesian_waypoints_planner_B.i_e = obj->ConstraintMatrix->size[0];
7190 cartesian_waypoints_planner_B.idx = alpha->size[0];
7191 alpha->size[0] = cartesian_waypoints_planner_B.i_e;
7192 cartes_emxEnsureCapacity_real_T(alpha,
7193 cartesian_waypoints_planner_B.idx);
7194 for (cartesian_waypoints_planner_B.idx = 0;
7195 cartesian_waypoints_planner_B.idx <
7196 cartesian_waypoints_planner_B.i_e;
7197 cartesian_waypoints_planner_B.idx++) {
7198 alpha->data[cartesian_waypoints_planner_B.idx] =
7199 obj->ConstraintMatrix->data
7200 [(cartesian_waypoints_planner_B.b_i_h - 1) *
7201 obj->ConstraintMatrix->size[0] +
7202 cartesian_waypoints_planner_B.idx];
7203 }
7204
7205 cartesian_waypoints_planner_B.b_i_h = alpha_0->size[0] *
7206 alpha_0->size[1];
7207 alpha_0->size[0] = 1;
7208 alpha_0->size[1] = alpha->size[0];
7209 cartes_emxEnsureCapacity_real_T(alpha_0,
7210 cartesian_waypoints_planner_B.b_i_h);
7211 cartesian_waypoints_planner_B.i_e = alpha->size[0];
7212 for (cartesian_waypoints_planner_B.b_i_h = 0;
7213 cartesian_waypoints_planner_B.b_i_h <
7214 cartesian_waypoints_planner_B.i_e;
7215 cartesian_waypoints_planner_B.b_i_h++) {
7216 alpha_0->data[cartesian_waypoints_planner_B.b_i_h] = alpha->
7217 data[cartesian_waypoints_planner_B.b_i_h];
7218 }
7219
7220 cartesian_waypoints_planner_B.s_p = 0.0;
7221 for (cartesian_waypoints_planner_B.b_i_h = 0;
7222 cartesian_waypoints_planner_B.b_i_h < 6;
7223 cartesian_waypoints_planner_B.b_i_h++) {
7224 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7225 = 0.0;
7226 for (cartesian_waypoints_planner_B.idx = 0;
7227 cartesian_waypoints_planner_B.idx < 6;
7228 cartesian_waypoints_planner_B.idx++) {
7229 cartesian_waypoints_planner_B.b_gamma =
7230 cartesian_waypoints_planner_B.P[6 *
7231 cartesian_waypoints_planner_B.b_i_h +
7232 cartesian_waypoints_planner_B.idx] * alpha_0->
7233 data[cartesian_waypoints_planner_B.idx] +
7234 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];
7235 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7236 = cartesian_waypoints_planner_B.b_gamma;
7237 }
7238
7239 cartesian_waypoints_planner_B.s_p +=
7240 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7241 * alpha->data[cartesian_waypoints_planner_B.b_i_h];
7242 }
7243
7244 cartesian_waypoints_planner_B.s_p = 1.0 /
7245 cartesian_waypoints_planner_B.s_p;
7246 for (cartesian_waypoints_planner_B.b_i_h = 0;
7247 cartesian_waypoints_planner_B.b_i_h < 36;
7248 cartesian_waypoints_planner_B.b_i_h++) {
7249 cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.b_i_h]
7250 = cartesian_waypoints_planner_B.s_p *
7251 cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.b_i_h];
7252 }
7253
7254 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
7255 AIn->size[0] = alpha->size[0];
7256 AIn->size[1] = alpha->size[0];
7257 cartes_emxEnsureCapacity_real_T(AIn,
7258 cartesian_waypoints_planner_B.b_i_h);
7259 cartesian_waypoints_planner_B.i_e = alpha->size[0];
7260 for (cartesian_waypoints_planner_B.b_i_h = 0;
7261 cartesian_waypoints_planner_B.b_i_h <
7262 cartesian_waypoints_planner_B.i_e;
7263 cartesian_waypoints_planner_B.b_i_h++) {
7264 cartesian_waypoints_planner_B.n_b = alpha->size[0];
7265 for (cartesian_waypoints_planner_B.idx = 0;
7266 cartesian_waypoints_planner_B.idx <
7267 cartesian_waypoints_planner_B.n_b;
7268 cartesian_waypoints_planner_B.idx++) {
7269 AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *
7270 cartesian_waypoints_planner_B.b_i_h] = alpha->
7271 data[cartesian_waypoints_planner_B.idx] * alpha->
7272 data[cartesian_waypoints_planner_B.b_i_h];
7273 }
7274 }
7275
7276 cartesian_waypoints_planner_B.n_b = AIn->size[1] - 1;
7277 cartesian_waypoints_planner_B.b_i_h = unusedU1->size[0] *
7278 unusedU1->size[1];
7279 unusedU1->size[0] = 6;
7280 unusedU1->size[1] = AIn->size[1];
7281 cartes_emxEnsureCapacity_real_T(unusedU1,
7282 cartesian_waypoints_planner_B.b_i_h);
7283 for (cartesian_waypoints_planner_B.idx = 0;
7284 cartesian_waypoints_planner_B.idx <=
7285 cartesian_waypoints_planner_B.n_b;
7286 cartesian_waypoints_planner_B.idx++) {
7287 cartesian_waypoints_planner_B.coffset =
7288 cartesian_waypoints_planner_B.idx * 6 - 1;
7289 cartesian_waypoints_planner_B.boffset =
7290 cartesian_waypoints_planner_B.idx * AIn->size[0] - 1;
7291 for (cartesian_waypoints_planner_B.b_i_h = 0;
7292 cartesian_waypoints_planner_B.b_i_h < 6;
7293 cartesian_waypoints_planner_B.b_i_h++) {
7294 cartesian_waypoints_planner_B.s_p = 0.0;
7295 for (cartesian_waypoints_planner_B.nx_j = 0;
7296 cartesian_waypoints_planner_B.nx_j < 6;
7297 cartesian_waypoints_planner_B.nx_j++) {
7298 cartesian_waypoints_planner_B.s_p +=
7299 cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.nx_j
7300 * 6 + cartesian_waypoints_planner_B.b_i_h] * AIn->data
7301 [(cartesian_waypoints_planner_B.boffset +
7302 cartesian_waypoints_planner_B.nx_j) + 1];
7303 }
7304
7305 unusedU1->data[(cartesian_waypoints_planner_B.coffset +
7306 cartesian_waypoints_planner_B.b_i_h) + 1] =
7307 cartesian_waypoints_planner_B.s_p;
7308 }
7309 }
7310
7311 for (cartesian_waypoints_planner_B.b_i_h = 0;
7312 cartesian_waypoints_planner_B.b_i_h < 6;
7313 cartesian_waypoints_planner_B.b_i_h++) {
7314 for (cartesian_waypoints_planner_B.idx = 0;
7315 cartesian_waypoints_planner_B.idx < 6;
7316 cartesian_waypoints_planner_B.idx++) {
7317 cartesian_waypoints_planner_B.s_p = 0.0;
7318 for (cartesian_waypoints_planner_B.i_e = 0;
7319 cartesian_waypoints_planner_B.i_e < 6;
7320 cartesian_waypoints_planner_B.i_e++) {
7321 cartesian_waypoints_planner_B.s_p += unusedU1->data[6 *
7322 cartesian_waypoints_planner_B.i_e +
7323 cartesian_waypoints_planner_B.b_i_h] *
7324 cartesian_waypoints_planner_B.P[6 *
7325 cartesian_waypoints_planner_B.idx +
7326 cartesian_waypoints_planner_B.i_e];
7327 }
7328
7329 cartesian_waypoints_planner_B.idxl = 6 *
7330 cartesian_waypoints_planner_B.idx +
7331 cartesian_waypoints_planner_B.b_i_h;
7332 cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.idxl]
7333 += cartesian_waypoints_planner_B.s_p;
7334 }
7335 }
7336
7337 cartesian_waypoints_planner_B.g_idx_0++;
7338 } else {
7339 guard2 = true;
7340 }
7341 } else {
7342 guard2 = true;
7343 }
7344
7345 if (guard2) {
7346 for (cartesian_waypoints_planner_B.b_i_h = 0;
7347 cartesian_waypoints_planner_B.b_i_h < 6;
7348 cartesian_waypoints_planner_B.b_i_h++) {
7349 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7350 =
7351 -cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];
7352 }
7353
7354 cartesian_waypoints_planner_B.idxl = -1;
7355 if (obj->ConstraintsOn) {
7356 cartesian_waypoints_planner_B.b_i_h = x->size[0];
7357 x->size[0] = activeSet->size[0];
7358 car_emxEnsureCapacity_boolean_T(x,
7359 cartesian_waypoints_planner_B.b_i_h);
7360 cartesian_waypoints_planner_B.i_e = activeSet->size[0];
7361 for (cartesian_waypoints_planner_B.b_i_h = 0;
7362 cartesian_waypoints_planner_B.b_i_h <
7363 cartesian_waypoints_planner_B.i_e;
7364 cartesian_waypoints_planner_B.b_i_h++) {
7365 x->data[cartesian_waypoints_planner_B.b_i_h] = !activeSet->
7366 data[cartesian_waypoints_planner_B.b_i_h];
7367 }
7368
7369 if (cartesian_waypoints_planner_any(x)) {
7370 cartesian_waypoints_planner_B.nx_j = activeSet->size[0] - 1;
7371 cartesian_waypoints_planner_B.idx = 0;
7372 for (cartesian_waypoints_planner_B.b_i_h = 0;
7373 cartesian_waypoints_planner_B.b_i_h <=
7374 cartesian_waypoints_planner_B.nx_j;
7375 cartesian_waypoints_planner_B.b_i_h++) {
7376 if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7377 cartesian_waypoints_planner_B.idx++;
7378 }
7379 }
7380
7381 cartesian_waypoints_planner_B.b_i_h = cb->size[0];
7382 cb->size[0] = cartesian_waypoints_planner_B.idx;
7383 carte_emxEnsureCapacity_int32_T(cb,
7384 cartesian_waypoints_planner_B.b_i_h);
7385 cartesian_waypoints_planner_B.idx = 0;
7386 for (cartesian_waypoints_planner_B.b_i_h = 0;
7387 cartesian_waypoints_planner_B.b_i_h <=
7388 cartesian_waypoints_planner_B.nx_j;
7389 cartesian_waypoints_planner_B.b_i_h++) {
7390 if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7391 cb->data[cartesian_waypoints_planner_B.idx] =
7392 cartesian_waypoints_planner_B.b_i_h + 1;
7393 cartesian_waypoints_planner_B.idx++;
7394 }
7395 }
7396
7397 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
7398 alpha->size[0] = cb->size[0];
7399 cartes_emxEnsureCapacity_real_T(alpha,
7400 cartesian_waypoints_planner_B.b_i_h);
7401 cartesian_waypoints_planner_B.i_e = cb->size[0];
7402 for (cartesian_waypoints_planner_B.b_i_h = 0;
7403 cartesian_waypoints_planner_B.b_i_h <
7404 cartesian_waypoints_planner_B.i_e;
7405 cartesian_waypoints_planner_B.b_i_h++) {
7406 alpha->data[cartesian_waypoints_planner_B.b_i_h] =
7407 obj->ConstraintBound->data[cb->
7408 data[cartesian_waypoints_planner_B.b_i_h] - 1];
7409 }
7410
7411 cartesian_waypoints_planner_B.nx_j = activeSet->size[0] - 1;
7412 cartesian_waypoints_planner_B.idx = 0;
7413 for (cartesian_waypoints_planner_B.b_i_h = 0;
7414 cartesian_waypoints_planner_B.b_i_h <=
7415 cartesian_waypoints_planner_B.nx_j;
7416 cartesian_waypoints_planner_B.b_i_h++) {
7417 if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7418 cartesian_waypoints_planner_B.idx++;
7419 }
7420 }
7421
7422 cartesian_waypoints_planner_B.b_i_h = db->size[0];
7423 db->size[0] = cartesian_waypoints_planner_B.idx;
7424 carte_emxEnsureCapacity_int32_T(db,
7425 cartesian_waypoints_planner_B.b_i_h);
7426 cartesian_waypoints_planner_B.idx = 0;
7427 for (cartesian_waypoints_planner_B.b_i_h = 0;
7428 cartesian_waypoints_planner_B.b_i_h <=
7429 cartesian_waypoints_planner_B.nx_j;
7430 cartesian_waypoints_planner_B.b_i_h++) {
7431 if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7432 db->data[cartesian_waypoints_planner_B.idx] =
7433 cartesian_waypoints_planner_B.b_i_h + 1;
7434 cartesian_waypoints_planner_B.idx++;
7435 }
7436 }
7437
7438 cartesian_waypoints_planner_B.i_e = obj->ConstraintMatrix->size
7439 [0];
7440 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
7441 AIn->size[0] = cartesian_waypoints_planner_B.i_e;
7442 AIn->size[1] = db->size[0];
7443 cartes_emxEnsureCapacity_real_T(AIn,
7444 cartesian_waypoints_planner_B.b_i_h);
7445 cartesian_waypoints_planner_B.n_b = db->size[0];
7446 for (cartesian_waypoints_planner_B.b_i_h = 0;
7447 cartesian_waypoints_planner_B.b_i_h <
7448 cartesian_waypoints_planner_B.n_b;
7449 cartesian_waypoints_planner_B.b_i_h++) {
7450 for (cartesian_waypoints_planner_B.idx = 0;
7451 cartesian_waypoints_planner_B.idx <
7452 cartesian_waypoints_planner_B.i_e;
7453 cartesian_waypoints_planner_B.idx++) {
7454 AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *
7455 cartesian_waypoints_planner_B.b_i_h] =
7456 obj->ConstraintMatrix->data[(db->
7457 data[cartesian_waypoints_planner_B.b_i_h] - 1) *
7458 obj->ConstraintMatrix->size[0] +
7459 cartesian_waypoints_planner_B.idx];
7460 }
7461 }
7462
7463 cartesian_waypoints_planner_B.nx_j = x->size[0];
7464 cartesian_waypoints_planner_B.idx = 0;
7465 cartesian_waypoints_planner_B.b_i_h = ii->size[0];
7466 ii->size[0] = x->size[0];
7467 carte_emxEnsureCapacity_int32_T(ii,
7468 cartesian_waypoints_planner_B.b_i_h);
7469 cartesian_waypoints_planner_B.b_i_h = 1;
7470 exitg3 = false;
7471 while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h - 1 <=
7472 cartesian_waypoints_planner_B.nx_j - 1)) {
7473 if (x->data[cartesian_waypoints_planner_B.b_i_h - 1]) {
7474 cartesian_waypoints_planner_B.idx++;
7475 ii->data[cartesian_waypoints_planner_B.idx - 1] =
7476 cartesian_waypoints_planner_B.b_i_h;
7477 if (cartesian_waypoints_planner_B.idx >=
7478 cartesian_waypoints_planner_B.nx_j) {
7479 exitg3 = true;
7480 } else {
7481 cartesian_waypoints_planner_B.b_i_h++;
7482 }
7483 } else {
7484 cartesian_waypoints_planner_B.b_i_h++;
7485 }
7486 }
7487
7488 if (x->size[0] == 1) {
7489 if (cartesian_waypoints_planner_B.idx == 0) {
7490 ii->size[0] = 0;
7491 }
7492 } else {
7493 if (1 > cartesian_waypoints_planner_B.idx) {
7494 cartesian_waypoints_planner_B.idx = 0;
7495 }
7496
7497 cartesian_waypoints_planner_B.b_i_h = ii_2->size[0];
7498 ii_2->size[0] = cartesian_waypoints_planner_B.idx;
7499 carte_emxEnsureCapacity_int32_T(ii_2,
7500 cartesian_waypoints_planner_B.b_i_h);
7501 for (cartesian_waypoints_planner_B.b_i_h = 0;
7502 cartesian_waypoints_planner_B.b_i_h <
7503 cartesian_waypoints_planner_B.idx;
7504 cartesian_waypoints_planner_B.b_i_h++) {
7505 ii_2->data[cartesian_waypoints_planner_B.b_i_h] = ii->
7506 data[cartesian_waypoints_planner_B.b_i_h];
7507 }
7508
7509 cartesian_waypoints_planner_B.b_i_h = ii->size[0];
7510 ii->size[0] = ii_2->size[0];
7511 carte_emxEnsureCapacity_int32_T(ii,
7512 cartesian_waypoints_planner_B.b_i_h);
7513 cartesian_waypoints_planner_B.i_e = ii_2->size[0];
7514 for (cartesian_waypoints_planner_B.b_i_h = 0;
7515 cartesian_waypoints_planner_B.b_i_h <
7516 cartesian_waypoints_planner_B.i_e;
7517 cartesian_waypoints_planner_B.b_i_h++) {
7518 ii->data[cartesian_waypoints_planner_B.b_i_h] = ii_2->
7519 data[cartesian_waypoints_planner_B.b_i_h];
7520 }
7521 }
7522
7523 cartesian_waypoints_planner_B.m_k = AIn->size[1] - 1;
7524 cartesian_waypoints_planner_B.inner = AIn->size[0] - 1;
7525 cartesian_waypoints_planner_B.b_i_h = L->size[0];
7526 L->size[0] = AIn->size[1];
7527 cartes_emxEnsureCapacity_real_T(L,
7528 cartesian_waypoints_planner_B.b_i_h);
7529 for (cartesian_waypoints_planner_B.b_i_h = 0;
7530 cartesian_waypoints_planner_B.b_i_h <=
7531 cartesian_waypoints_planner_B.m_k;
7532 cartesian_waypoints_planner_B.b_i_h++) {
7533 L->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;
7534 }
7535
7536 for (cartesian_waypoints_planner_B.nx_j = 0;
7537 cartesian_waypoints_planner_B.nx_j <=
7538 cartesian_waypoints_planner_B.inner;
7539 cartesian_waypoints_planner_B.nx_j++) {
7540 for (cartesian_waypoints_planner_B.j = 0;
7541 cartesian_waypoints_planner_B.j <=
7542 cartesian_waypoints_planner_B.m_k;
7543 cartesian_waypoints_planner_B.j++) {
7544 L->data[cartesian_waypoints_planner_B.j] += AIn->
7545 data[cartesian_waypoints_planner_B.j * AIn->size[0] +
7546 cartesian_waypoints_planner_B.nx_j] *
7547 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.nx_j];
7548 }
7549 }
7550
7551 cartesian_waypoints_planner_B.m_k = AIn->size[1] - 1;
7552 cartesian_waypoints_planner_B.inner = AIn->size[0] - 1;
7553 cartesian_waypoints_planner_B.b_i_h = y->size[0];
7554 y->size[0] = AIn->size[1];
7555 cartes_emxEnsureCapacity_real_T(y,
7556 cartesian_waypoints_planner_B.b_i_h);
7557 for (cartesian_waypoints_planner_B.b_i_h = 0;
7558 cartesian_waypoints_planner_B.b_i_h <=
7559 cartesian_waypoints_planner_B.m_k;
7560 cartesian_waypoints_planner_B.b_i_h++) {
7561 y->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;
7562 }
7563
7564 for (cartesian_waypoints_planner_B.nx_j = 0;
7565 cartesian_waypoints_planner_B.nx_j <=
7566 cartesian_waypoints_planner_B.inner;
7567 cartesian_waypoints_planner_B.nx_j++) {
7568 for (cartesian_waypoints_planner_B.j = 0;
7569 cartesian_waypoints_planner_B.j <=
7570 cartesian_waypoints_planner_B.m_k;
7571 cartesian_waypoints_planner_B.j++) {
7572 y->data[cartesian_waypoints_planner_B.j] += AIn->
7573 data[cartesian_waypoints_planner_B.j * AIn->size[0] +
7574 cartesian_waypoints_planner_B.nx_j] *
7575 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.nx_j];
7576 }
7577 }
7578
7579 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
7580 cartes_emxEnsureCapacity_real_T(alpha,
7581 cartesian_waypoints_planner_B.b_i_h);
7582 cartesian_waypoints_planner_B.i_e = alpha->size[0];
7583 for (cartesian_waypoints_planner_B.b_i_h = 0;
7584 cartesian_waypoints_planner_B.b_i_h <
7585 cartesian_waypoints_planner_B.i_e;
7586 cartesian_waypoints_planner_B.b_i_h++) {
7587 alpha->data[cartesian_waypoints_planner_B.b_i_h] =
7588 (alpha->data[cartesian_waypoints_planner_B.b_i_h] - L->
7589 data[cartesian_waypoints_planner_B.b_i_h]) / y->
7590 data[cartesian_waypoints_planner_B.b_i_h];
7591 }
7592
7593 cartesian_waypoints_planner_B.b_i_h = x->size[0];
7594 x->size[0] = alpha->size[0];
7595 car_emxEnsureCapacity_boolean_T(x,
7596 cartesian_waypoints_planner_B.b_i_h);
7597 cartesian_waypoints_planner_B.i_e = alpha->size[0];
7598 for (cartesian_waypoints_planner_B.b_i_h = 0;
7599 cartesian_waypoints_planner_B.b_i_h <
7600 cartesian_waypoints_planner_B.i_e;
7601 cartesian_waypoints_planner_B.b_i_h++) {
7602 x->data[cartesian_waypoints_planner_B.b_i_h] = (alpha->
7603 data[cartesian_waypoints_planner_B.b_i_h] > 0.0);
7604 }
7605
7606 cartesian_waypoints_planner_B.nx_j = x->size[0];
7607 cartesian_waypoints_planner_B.idx = 0;
7608 cartesian_waypoints_planner_B.b_i_h = ii_0->size[0];
7609 ii_0->size[0] = x->size[0];
7610 carte_emxEnsureCapacity_int32_T(ii_0,
7611 cartesian_waypoints_planner_B.b_i_h);
7612 cartesian_waypoints_planner_B.b_i_h = 1;
7613 exitg3 = false;
7614 while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h - 1 <=
7615 cartesian_waypoints_planner_B.nx_j - 1)) {
7616 if (x->data[cartesian_waypoints_planner_B.b_i_h - 1]) {
7617 cartesian_waypoints_planner_B.idx++;
7618 ii_0->data[cartesian_waypoints_planner_B.idx - 1] =
7619 cartesian_waypoints_planner_B.b_i_h;
7620 if (cartesian_waypoints_planner_B.idx >=
7621 cartesian_waypoints_planner_B.nx_j) {
7622 exitg3 = true;
7623 } else {
7624 cartesian_waypoints_planner_B.b_i_h++;
7625 }
7626 } else {
7627 cartesian_waypoints_planner_B.b_i_h++;
7628 }
7629 }
7630
7631 if (x->size[0] == 1) {
7632 if (cartesian_waypoints_planner_B.idx == 0) {
7633 ii_0->size[0] = 0;
7634 }
7635 } else {
7636 if (1 > cartesian_waypoints_planner_B.idx) {
7637 cartesian_waypoints_planner_B.idx = 0;
7638 }
7639
7640 cartesian_waypoints_planner_B.b_i_h = ii_3->size[0];
7641 ii_3->size[0] = cartesian_waypoints_planner_B.idx;
7642 carte_emxEnsureCapacity_int32_T(ii_3,
7643 cartesian_waypoints_planner_B.b_i_h);
7644 for (cartesian_waypoints_planner_B.b_i_h = 0;
7645 cartesian_waypoints_planner_B.b_i_h <
7646 cartesian_waypoints_planner_B.idx;
7647 cartesian_waypoints_planner_B.b_i_h++) {
7648 ii_3->data[cartesian_waypoints_planner_B.b_i_h] = ii_0->
7649 data[cartesian_waypoints_planner_B.b_i_h];
7650 }
7651
7652 cartesian_waypoints_planner_B.b_i_h = ii_0->size[0];
7653 ii_0->size[0] = ii_3->size[0];
7654 carte_emxEnsureCapacity_int32_T(ii_0,
7655 cartesian_waypoints_planner_B.b_i_h);
7656 cartesian_waypoints_planner_B.i_e = ii_3->size[0];
7657 for (cartesian_waypoints_planner_B.b_i_h = 0;
7658 cartesian_waypoints_planner_B.b_i_h <
7659 cartesian_waypoints_planner_B.i_e;
7660 cartesian_waypoints_planner_B.b_i_h++) {
7661 ii_0->data[cartesian_waypoints_planner_B.b_i_h] = ii_3->
7662 data[cartesian_waypoints_planner_B.b_i_h];
7663 }
7664 }
7665
7666 cartesian_waypoints_planner_B.b_i_h = L->size[0];
7667 L->size[0] = ii_0->size[0];
7668 cartes_emxEnsureCapacity_real_T(L,
7669 cartesian_waypoints_planner_B.b_i_h);
7670 cartesian_waypoints_planner_B.i_e = ii_0->size[0];
7671 for (cartesian_waypoints_planner_B.b_i_h = 0;
7672 cartesian_waypoints_planner_B.b_i_h <
7673 cartesian_waypoints_planner_B.i_e;
7674 cartesian_waypoints_planner_B.b_i_h++) {
7675 L->data[cartesian_waypoints_planner_B.b_i_h] = ii_0->
7676 data[cartesian_waypoints_planner_B.b_i_h];
7677 }
7678
7679 if (L->size[0] != 0) {
7680 cartesian_waypoints_planner_B.nx_j = alpha->size[0] - 1;
7681 cartesian_waypoints_planner_B.idx = 0;
7682 for (cartesian_waypoints_planner_B.b_i_h = 0;
7683 cartesian_waypoints_planner_B.b_i_h <=
7684 cartesian_waypoints_planner_B.nx_j;
7685 cartesian_waypoints_planner_B.b_i_h++) {
7686 if (alpha->data[cartesian_waypoints_planner_B.b_i_h] > 0.0)
7687 {
7688 cartesian_waypoints_planner_B.idx++;
7689 }
7690 }
7691
7692 cartesian_waypoints_planner_B.b_i_h = fb->size[0];
7693 fb->size[0] = cartesian_waypoints_planner_B.idx;
7694 carte_emxEnsureCapacity_int32_T(fb,
7695 cartesian_waypoints_planner_B.b_i_h);
7696 cartesian_waypoints_planner_B.idx = 0;
7697 for (cartesian_waypoints_planner_B.b_i_h = 0;
7698 cartesian_waypoints_planner_B.b_i_h <=
7699 cartesian_waypoints_planner_B.nx_j;
7700 cartesian_waypoints_planner_B.b_i_h++) {
7701 if (alpha->data[cartesian_waypoints_planner_B.b_i_h] > 0.0)
7702 {
7703 fb->data[cartesian_waypoints_planner_B.idx] =
7704 cartesian_waypoints_planner_B.b_i_h + 1;
7705 cartesian_waypoints_planner_B.idx++;
7706 }
7707 }
7708
7709 cartesian_waypoints_planner_B.n_b = fb->size[0];
7710 if (fb->size[0] <= 2) {
7711 if (fb->size[0] == 1) {
7712 cartesian_waypoints_planner_B.s_p = alpha->data[fb->data[0]
7713 - 1];
7714 cartesian_waypoints_planner_B.idxl = 0;
7715 } else if ((alpha->data[fb->data[0] - 1] > alpha->data
7716 [fb->data[1] - 1]) || (rtIsNaN(alpha->data
7717 [fb->data[0] - 1]) && (!rtIsNaN(alpha->data
7718 [fb->data[1] - 1])))) {
7719 cartesian_waypoints_planner_B.s_p = alpha->data[fb->data[1]
7720 - 1];
7721 cartesian_waypoints_planner_B.idxl = 1;
7722 } else {
7723 cartesian_waypoints_planner_B.s_p = alpha->data[fb->data[0]
7724 - 1];
7725 cartesian_waypoints_planner_B.idxl = 0;
7726 }
7727 } else {
7728 if (!rtIsNaN(alpha->data[fb->data[0] - 1])) {
7729 cartesian_waypoints_planner_B.idxl = 1;
7730 } else {
7731 cartesian_waypoints_planner_B.idxl = 0;
7732 cartesian_waypoints_planner_B.b_i_h = 2;
7733 exitg3 = false;
7734 while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h <=
7735 fb->size[0])) {
7736 if (!rtIsNaN(alpha->data[fb->
7737 data[cartesian_waypoints_planner_B.b_i_h -
7738 1] - 1])) {
7739 cartesian_waypoints_planner_B.idxl =
7740 cartesian_waypoints_planner_B.b_i_h;
7741 exitg3 = true;
7742 } else {
7743 cartesian_waypoints_planner_B.b_i_h++;
7744 }
7745 }
7746 }
7747
7748 if (cartesian_waypoints_planner_B.idxl == 0) {
7749 cartesian_waypoints_planner_B.s_p = alpha->data[fb->data[0]
7750 - 1];
7751 } else {
7752 cartesian_waypoints_planner_B.s_p = alpha->data[fb->
7753 data[cartesian_waypoints_planner_B.idxl - 1] - 1];
7754 cartesian_waypoints_planner_B.nx_j =
7755 cartesian_waypoints_planner_B.idxl;
7756 for (cartesian_waypoints_planner_B.b_i_h =
7757 cartesian_waypoints_planner_B.idxl + 1;
7758 cartesian_waypoints_planner_B.b_i_h <=
7759 cartesian_waypoints_planner_B.n_b;
7760 cartesian_waypoints_planner_B.b_i_h++) {
7761 if (cartesian_waypoints_planner_B.s_p > alpha->data
7762 [fb->data[cartesian_waypoints_planner_B.b_i_h - 1] -
7763 1]) {
7764 cartesian_waypoints_planner_B.s_p = alpha->data
7765 [fb->data[cartesian_waypoints_planner_B.b_i_h - 1] -
7766 1];
7767 cartesian_waypoints_planner_B.nx_j =
7768 cartesian_waypoints_planner_B.b_i_h;
7769 }
7770 }
7771
7772 cartesian_waypoints_planner_B.idxl =
7773 cartesian_waypoints_planner_B.nx_j - 1;
7774 }
7775 }
7776
7777 cartesian_waypoints_planner_B.idxl = ii->data
7778 [static_cast<int32_T>(L->
7779 data[cartesian_waypoints_planner_B.idxl]) - 1];
7780 } else {
7781 cartesian_waypoints_planner_B.s_p = 0.0;
7782 }
7783 } else {
7784 cartesian_waypoints_planner_B.s_p = 0.0;
7785 }
7786 } else {
7787 cartesian_waypoints_planner_B.s_p = 0.0;
7788 }
7789
7790 if (cartesian_waypoints_planner_B.s_p > 0.0) {
7791 if (1.0 < cartesian_waypoints_planner_B.s_p) {
7792 cartesian_waypoints_planner_B.b_gamma = 1.0;
7793 } else {
7794 cartesian_waypoints_planner_B.b_gamma =
7795 cartesian_waypoints_planner_B.s_p;
7796 }
7797 } else {
7798 cartesian_waypoints_planner_B.b_gamma = 1.0;
7799 }
7800
7801 cartesian_waypoints_planner_B.beta = obj->ArmijoRuleBeta;
7802 cartesian_waypoints_planner_B.sigma = obj->ArmijoRuleSigma;
7803 for (cartesian_waypoints_planner_B.b_i_h = 0;
7804 cartesian_waypoints_planner_B.b_i_h < 6;
7805 cartesian_waypoints_planner_B.b_i_h++) {
7806 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
7807 = cartesian_waypoints_planner_B.b_gamma *
7808 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7809 + cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
7810 }
7811
7812 cartesian_IKHelpers_computeCost(cartesian_waypoints_planner_B.sNew_p,
7813 obj->ExtraArgs, &cartesian_waypoints_planner_B.costNew,
7814 cartesian_waypoints_planner_B.V, unusedU1, &c);
7815 obj->ExtraArgs = c;
7816 cartesian_waypoints_planner_B.m = 0.0;
7817 do {
7818 exitg1 = 0;
7819 for (cartesian_waypoints_planner_B.i_e = 0;
7820 cartesian_waypoints_planner_B.i_e < 6;
7821 cartesian_waypoints_planner_B.i_e++) {
7822 xSol[cartesian_waypoints_planner_B.i_e] =
7823 cartesian_waypoints_planner_B.b_gamma *
7824 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.i_e];
7825 }
7826
7827 cartesian_waypoints_planner_B.b_i_h = sigma->size[0] * sigma->
7828 size[1];
7829 sigma->size[0] = 1;
7830 sigma->size[1] = grad->size[0];
7831 cartes_emxEnsureCapacity_real_T(sigma,
7832 cartesian_waypoints_planner_B.b_i_h);
7833 cartesian_waypoints_planner_B.i_e = grad->size[0];
7834 for (cartesian_waypoints_planner_B.b_i_h = 0;
7835 cartesian_waypoints_planner_B.b_i_h <
7836 cartesian_waypoints_planner_B.i_e;
7837 cartesian_waypoints_planner_B.b_i_h++) {
7838 sigma->data[cartesian_waypoints_planner_B.b_i_h] =
7839 -cartesian_waypoints_planner_B.sigma * grad->
7840 data[cartesian_waypoints_planner_B.b_i_h];
7841 }
7842
7843 cartesian_waypoints_planner_B.sigma_g = 0.0;
7844 for (cartesian_waypoints_planner_B.b_i_h = 0;
7845 cartesian_waypoints_planner_B.b_i_h < 6;
7846 cartesian_waypoints_planner_B.b_i_h++) {
7847 cartesian_waypoints_planner_B.sigma_g += sigma->
7848 data[cartesian_waypoints_planner_B.b_i_h] *
7849 xSol[cartesian_waypoints_planner_B.b_i_h];
7850 }
7851
7852 if (cartesian_waypoints_planner_B.cost -
7853 cartesian_waypoints_planner_B.costNew <
7854 cartesian_waypoints_planner_B.sigma_g) {
7855 cartesian_waypoints_planner_B.flag =
7856 (cartesian_waypoints_planner_B.b_gamma < obj->StepTolerance);
7857 if (cartesian_waypoints_planner_B.flag) {
7858 for (cartesian_waypoints_planner_B.i_e = 0;
7859 cartesian_waypoints_planner_B.i_e < 6;
7860 cartesian_waypoints_planner_B.i_e++) {
7861 xSol[cartesian_waypoints_planner_B.i_e] =
7862 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_e];
7863 }
7864
7865 *exitFlag = StepSizeBelowMinimum;
7866 args = obj->ExtraArgs;
7867 for (cartesian_waypoints_planner_B.b_i_h = 0;
7868 cartesian_waypoints_planner_B.b_i_h < 36;
7869 cartesian_waypoints_planner_B.b_i_h++) {
7870 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
7871 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
7872 }
7873
7874 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
7875 grad->size[0] = args->ErrTemp->size[0];
7876 cartes_emxEnsureCapacity_real_T(grad,
7877 cartesian_waypoints_planner_B.b_i_h);
7878 cartesian_waypoints_planner_B.i_e = args->ErrTemp->size[0];
7879 for (cartesian_waypoints_planner_B.b_i_h = 0;
7880 cartesian_waypoints_planner_B.b_i_h <
7881 cartesian_waypoints_planner_B.i_e;
7882 cartesian_waypoints_planner_B.b_i_h++) {
7883 grad->data[cartesian_waypoints_planner_B.b_i_h] =
7884 args->ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];
7885 }
7886
7887 for (cartesian_waypoints_planner_B.b_i_h = 0;
7888 cartesian_waypoints_planner_B.b_i_h < 6;
7889 cartesian_waypoints_planner_B.b_i_h++) {
7890 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
7891 = 0.0;
7892 for (cartesian_waypoints_planner_B.idx = 0;
7893 cartesian_waypoints_planner_B.idx < 6;
7894 cartesian_waypoints_planner_B.idx++) {
7895 cartesian_waypoints_planner_B.A_d =
7896 cartesian_waypoints_planner_B.unusedU0[6 *
7897 cartesian_waypoints_planner_B.idx +
7898 cartesian_waypoints_planner_B.b_i_h] * grad->
7899 data[cartesian_waypoints_planner_B.idx] +
7900 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
7901 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
7902 = cartesian_waypoints_planner_B.A_d;
7903 }
7904 }
7905
7906 *err = cartesian_waypoints_plan_norm_e
7907 (cartesian_waypoints_planner_B.x);
7908 *iter = static_cast<real_T>
7909 (cartesian_waypoints_planner_B.g_idx_0) + 1.0;
7910 exitg1 = 1;
7911 } else {
7912 cartesian_waypoints_planner_B.b_gamma *=
7913 cartesian_waypoints_planner_B.beta;
7914 cartesian_waypoints_planner_B.m++;
7915 for (cartesian_waypoints_planner_B.b_i_h = 0;
7916 cartesian_waypoints_planner_B.b_i_h < 6;
7917 cartesian_waypoints_planner_B.b_i_h++) {
7918 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
7919 = cartesian_waypoints_planner_B.b_gamma *
7920 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
7921 + cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
7922 }
7923
7924 cartesian_IKHelpers_computeCost
7925 (cartesian_waypoints_planner_B.sNew_p, obj->ExtraArgs,
7926 &cartesian_waypoints_planner_B.costNew,
7927 cartesian_waypoints_planner_B.V, unusedU1, &d);
7928 obj->ExtraArgs = d;
7929 }
7930 } else {
7931 for (cartesian_waypoints_planner_B.b_i_h = 0;
7932 cartesian_waypoints_planner_B.b_i_h < 6;
7933 cartesian_waypoints_planner_B.b_i_h++) {
7934 xSol[cartesian_waypoints_planner_B.b_i_h] +=
7935 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
7936 }
7937
7938 args = obj->ExtraArgs;
7939 cartesian_waypoints_planner_B.b_i_h = alpha->size[0];
7940 alpha->size[0] = args->GradTemp->size[0];
7941 cartes_emxEnsureCapacity_real_T(alpha,
7942 cartesian_waypoints_planner_B.b_i_h);
7943 cartesian_waypoints_planner_B.i_e = args->GradTemp->size[0];
7944 for (cartesian_waypoints_planner_B.b_i_h = 0;
7945 cartesian_waypoints_planner_B.b_i_h <
7946 cartesian_waypoints_planner_B.i_e;
7947 cartesian_waypoints_planner_B.b_i_h++) {
7948 alpha->data[cartesian_waypoints_planner_B.b_i_h] =
7949 args->GradTemp->data[cartesian_waypoints_planner_B.b_i_h];
7950 }
7951
7952 exitg1 = 2;
7953 }
7954 } while (exitg1 == 0);
7955
7956 if (exitg1 == 1) {
7957 exitg2 = 1;
7958 } else if ((cartesian_waypoints_planner_B.m == 0.0) && (fabs
7959 (cartesian_waypoints_planner_B.b_gamma -
7960 cartesian_waypoints_planner_B.s_p) <
7961 1.4901161193847656E-8)) {
7962 cartesian_waypoints_planner_B.i_e = obj->ConstraintMatrix->size[0];
7963 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
7964 grad->size[0] = cartesian_waypoints_planner_B.i_e;
7965 cartes_emxEnsureCapacity_real_T(grad,
7966 cartesian_waypoints_planner_B.b_i_h);
7967 for (cartesian_waypoints_planner_B.b_i_h = 0;
7968 cartesian_waypoints_planner_B.b_i_h <
7969 cartesian_waypoints_planner_B.i_e;
7970 cartesian_waypoints_planner_B.b_i_h++) {
7971 grad->data[cartesian_waypoints_planner_B.b_i_h] =
7972 obj->ConstraintMatrix->data
7973 [(cartesian_waypoints_planner_B.idxl - 1) *
7974 obj->ConstraintMatrix->size[0] +
7975 cartesian_waypoints_planner_B.b_i_h];
7976 }
7977
7978 activeSet->data[cartesian_waypoints_planner_B.idxl - 1] = true;
7979 cartesian_waypoints_planner_B.nx_j = activeSet->size[0] - 1;
7980 cartesian_waypoints_planner_B.idx = 0;
7981 for (cartesian_waypoints_planner_B.b_i_h = 0;
7982 cartesian_waypoints_planner_B.b_i_h <=
7983 cartesian_waypoints_planner_B.nx_j;
7984 cartesian_waypoints_planner_B.b_i_h++) {
7985 if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
7986 cartesian_waypoints_planner_B.idx++;
7987 }
7988 }
7989
7990 cartesian_waypoints_planner_B.b_i_h = gb->size[0];
7991 gb->size[0] = cartesian_waypoints_planner_B.idx;
7992 carte_emxEnsureCapacity_int32_T(gb,
7993 cartesian_waypoints_planner_B.b_i_h);
7994 cartesian_waypoints_planner_B.idx = 0;
7995 for (cartesian_waypoints_planner_B.b_i_h = 0;
7996 cartesian_waypoints_planner_B.b_i_h <=
7997 cartesian_waypoints_planner_B.nx_j;
7998 cartesian_waypoints_planner_B.b_i_h++) {
7999 if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {
8000 gb->data[cartesian_waypoints_planner_B.idx] =
8001 cartesian_waypoints_planner_B.b_i_h + 1;
8002 cartesian_waypoints_planner_B.idx++;
8003 }
8004 }
8005
8006 cartesian_waypoints_planner_B.i_e = obj->ConstraintMatrix->size[0];
8007 cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];
8008 A->size[0] = cartesian_waypoints_planner_B.i_e;
8009 A->size[1] = gb->size[0];
8010 cartes_emxEnsureCapacity_real_T(A,
8011 cartesian_waypoints_planner_B.b_i_h);
8012 cartesian_waypoints_planner_B.n_b = gb->size[0];
8013 for (cartesian_waypoints_planner_B.b_i_h = 0;
8014 cartesian_waypoints_planner_B.b_i_h <
8015 cartesian_waypoints_planner_B.n_b;
8016 cartesian_waypoints_planner_B.b_i_h++) {
8017 for (cartesian_waypoints_planner_B.idx = 0;
8018 cartesian_waypoints_planner_B.idx <
8019 cartesian_waypoints_planner_B.i_e;
8020 cartesian_waypoints_planner_B.idx++) {
8021 A->data[cartesian_waypoints_planner_B.idx + A->size[0] *
8022 cartesian_waypoints_planner_B.b_i_h] = obj->
8023 ConstraintMatrix->data[(gb->
8024 data[cartesian_waypoints_planner_B.b_i_h] - 1) *
8025 obj->ConstraintMatrix->size[0] +
8026 cartesian_waypoints_planner_B.idx];
8027 }
8028 }
8029
8030 cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];
8031 AIn->size[0] = grad->size[0];
8032 AIn->size[1] = grad->size[0];
8033 cartes_emxEnsureCapacity_real_T(AIn,
8034 cartesian_waypoints_planner_B.b_i_h);
8035 cartesian_waypoints_planner_B.i_e = grad->size[0];
8036 for (cartesian_waypoints_planner_B.b_i_h = 0;
8037 cartesian_waypoints_planner_B.b_i_h <
8038 cartesian_waypoints_planner_B.i_e;
8039 cartesian_waypoints_planner_B.b_i_h++) {
8040 cartesian_waypoints_planner_B.n_b = grad->size[0];
8041 for (cartesian_waypoints_planner_B.idx = 0;
8042 cartesian_waypoints_planner_B.idx <
8043 cartesian_waypoints_planner_B.n_b;
8044 cartesian_waypoints_planner_B.idx++) {
8045 AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *
8046 cartesian_waypoints_planner_B.b_i_h] = grad->
8047 data[cartesian_waypoints_planner_B.idx] * grad->
8048 data[cartesian_waypoints_planner_B.b_i_h];
8049 }
8050 }
8051
8052 cartesian_waypoints_planner_B.m_k = AIn->size[0] - 1;
8053 cartesian_waypoints_planner_B.inner = AIn->size[1] - 1;
8054 cartesian_waypoints_planner_B.b_i_h = y_0->size[0] * y_0->size[1];
8055 y_0->size[0] = AIn->size[0];
8056 y_0->size[1] = 6;
8057 cartes_emxEnsureCapacity_real_T(y_0,
8058 cartesian_waypoints_planner_B.b_i_h);
8059 for (cartesian_waypoints_planner_B.idx = 0;
8060 cartesian_waypoints_planner_B.idx < 6;
8061 cartesian_waypoints_planner_B.idx++) {
8062 cartesian_waypoints_planner_B.coffset =
8063 (cartesian_waypoints_planner_B.m_k + 1) *
8064 cartesian_waypoints_planner_B.idx - 1;
8065 cartesian_waypoints_planner_B.boffset =
8066 cartesian_waypoints_planner_B.idx * 6 - 1;
8067 for (cartesian_waypoints_planner_B.b_i_h = 0;
8068 cartesian_waypoints_planner_B.b_i_h <=
8069 cartesian_waypoints_planner_B.m_k;
8070 cartesian_waypoints_planner_B.b_i_h++) {
8071 y_0->data[(cartesian_waypoints_planner_B.coffset +
8072 cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;
8073 }
8074
8075 for (cartesian_waypoints_planner_B.nx_j = 0;
8076 cartesian_waypoints_planner_B.nx_j <=
8077 cartesian_waypoints_planner_B.inner;
8078 cartesian_waypoints_planner_B.nx_j++) {
8079 cartesian_waypoints_planner_B.aoffset =
8080 cartesian_waypoints_planner_B.nx_j * AIn->size[0] - 1;
8081 cartesian_waypoints_planner_B.s_p =
8082 cartesian_waypoints_planner_B.H
8083 [(cartesian_waypoints_planner_B.boffset +
8084 cartesian_waypoints_planner_B.nx_j) + 1];
8085 for (cartesian_waypoints_planner_B.j = 0;
8086 cartesian_waypoints_planner_B.j <=
8087 cartesian_waypoints_planner_B.m_k;
8088 cartesian_waypoints_planner_B.j++) {
8089 cartesian_waypoints_planner_B.i_e =
8090 cartesian_waypoints_planner_B.j + 1;
8091 cartesian_waypoints_planner_B.b_i_h =
8092 cartesian_waypoints_planner_B.coffset +
8093 cartesian_waypoints_planner_B.i_e;
8094 y_0->data[cartesian_waypoints_planner_B.b_i_h] += AIn->
8095 data[cartesian_waypoints_planner_B.aoffset +
8096 cartesian_waypoints_planner_B.i_e] *
8097 cartesian_waypoints_planner_B.s_p;
8098 }
8099 }
8100 }
8101
8102 cartesian_waypoints_planner_B.b_i_h = grad_1->size[0] *
8103 grad_1->size[1];
8104 grad_1->size[0] = 1;
8105 grad_1->size[1] = grad->size[0];
8106 cartes_emxEnsureCapacity_real_T(grad_1,
8107 cartesian_waypoints_planner_B.b_i_h);
8108 cartesian_waypoints_planner_B.i_e = grad->size[0];
8109 for (cartesian_waypoints_planner_B.b_i_h = 0;
8110 cartesian_waypoints_planner_B.b_i_h <
8111 cartesian_waypoints_planner_B.i_e;
8112 cartesian_waypoints_planner_B.b_i_h++) {
8113 grad_1->data[cartesian_waypoints_planner_B.b_i_h] = grad->
8114 data[cartesian_waypoints_planner_B.b_i_h];
8115 }
8116
8117 cartesian_waypoints_planner_B.beta = 0.0;
8118 for (cartesian_waypoints_planner_B.b_i_h = 0;
8119 cartesian_waypoints_planner_B.b_i_h < 6;
8120 cartesian_waypoints_planner_B.b_i_h++) {
8121 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8122 = 0.0;
8123 for (cartesian_waypoints_planner_B.idx = 0;
8124 cartesian_waypoints_planner_B.idx < 6;
8125 cartesian_waypoints_planner_B.idx++) {
8126 cartesian_waypoints_planner_B.sigma =
8127 cartesian_waypoints_planner_B.H[6 *
8128 cartesian_waypoints_planner_B.b_i_h +
8129 cartesian_waypoints_planner_B.idx] * grad_1->
8130 data[cartesian_waypoints_planner_B.idx] +
8131 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h];
8132 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8133 = cartesian_waypoints_planner_B.sigma;
8134 }
8135
8136 cartesian_waypoints_planner_B.beta +=
8137 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8138 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8139 }
8140
8141 cartesian_waypoints_planner_B.s_p = 1.0 /
8142 cartesian_waypoints_planner_B.beta;
8143 for (cartesian_waypoints_planner_B.b_i_h = 0;
8144 cartesian_waypoints_planner_B.b_i_h < 6;
8145 cartesian_waypoints_planner_B.b_i_h++) {
8146 for (cartesian_waypoints_planner_B.idx = 0;
8147 cartesian_waypoints_planner_B.idx < 6;
8148 cartesian_waypoints_planner_B.idx++) {
8149 cartesian_waypoints_planner_B.idxl =
8150 cartesian_waypoints_planner_B.b_i_h + 6 *
8151 cartesian_waypoints_planner_B.idx;
8152 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.idxl]
8153 = 0.0;
8154 for (cartesian_waypoints_planner_B.i_e = 0;
8155 cartesian_waypoints_planner_B.i_e < 6;
8156 cartesian_waypoints_planner_B.i_e++) {
8157 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.idxl]
8158 += cartesian_waypoints_planner_B.H[6 *
8159 cartesian_waypoints_planner_B.i_e +
8160 cartesian_waypoints_planner_B.b_i_h] * y_0->data[6 *
8161 cartesian_waypoints_planner_B.idx +
8162 cartesian_waypoints_planner_B.i_e];
8163 }
8164 }
8165 }
8166
8167 for (cartesian_waypoints_planner_B.b_i_h = 0;
8168 cartesian_waypoints_planner_B.b_i_h < 36;
8169 cartesian_waypoints_planner_B.b_i_h++) {
8170 cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.b_i_h]
8171 -= cartesian_waypoints_planner_B.s_p *
8172 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.b_i_h];
8173 }
8174
8175 guard1 = true;
8176 } else {
8177 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
8178 grad->size[0] = alpha->size[0];
8179 cartes_emxEnsureCapacity_real_T(grad,
8180 cartesian_waypoints_planner_B.b_i_h);
8181 cartesian_waypoints_planner_B.i_e = alpha->size[0];
8182 for (cartesian_waypoints_planner_B.b_i_h = 0;
8183 cartesian_waypoints_planner_B.b_i_h <
8184 cartesian_waypoints_planner_B.i_e;
8185 cartesian_waypoints_planner_B.b_i_h++) {
8186 grad->data[cartesian_waypoints_planner_B.b_i_h] = alpha->
8187 data[cartesian_waypoints_planner_B.b_i_h] - grad->
8188 data[cartesian_waypoints_planner_B.b_i_h];
8189 }
8190
8191 cartesian_waypoints_planner_B.b_gamma = 0.0;
8192 for (cartesian_waypoints_planner_B.b_i_h = 0;
8193 cartesian_waypoints_planner_B.b_i_h < 6;
8194 cartesian_waypoints_planner_B.b_i_h++) {
8195 cartesian_waypoints_planner_B.b_gamma +=
8196 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
8197 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8198 }
8199
8200 cartesian_waypoints_planner_B.b_i_h = tmp->size[0] * tmp->size[1];
8201 tmp->size[0] = 1;
8202 tmp->size[1] = grad->size[0];
8203 cartes_emxEnsureCapacity_real_T(tmp,
8204 cartesian_waypoints_planner_B.b_i_h);
8205 cartesian_waypoints_planner_B.i_e = grad->size[0];
8206 for (cartesian_waypoints_planner_B.b_i_h = 0;
8207 cartesian_waypoints_planner_B.b_i_h <
8208 cartesian_waypoints_planner_B.i_e;
8209 cartesian_waypoints_planner_B.b_i_h++) {
8210 tmp->data[cartesian_waypoints_planner_B.b_i_h] = 0.2 *
8211 grad->data[cartesian_waypoints_planner_B.b_i_h];
8212 }
8213
8214 cartesian_waypoints_planner_B.s_p = 0.0;
8215 for (cartesian_waypoints_planner_B.b_i_h = 0;
8216 cartesian_waypoints_planner_B.b_i_h < 6;
8217 cartesian_waypoints_planner_B.b_i_h++) {
8218 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8219 = 0.0;
8220 for (cartesian_waypoints_planner_B.idx = 0;
8221 cartesian_waypoints_planner_B.idx < 6;
8222 cartesian_waypoints_planner_B.idx++) {
8223 cartesian_waypoints_planner_B.beta =
8224 cartesian_waypoints_planner_B.H[6 *
8225 cartesian_waypoints_planner_B.b_i_h +
8226 cartesian_waypoints_planner_B.idx] * tmp->
8227 data[cartesian_waypoints_planner_B.idx] +
8228 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h];
8229 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8230 = cartesian_waypoints_planner_B.beta;
8231 }
8232
8233 cartesian_waypoints_planner_B.s_p +=
8234 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8235 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8236 }
8237
8238 if (cartesian_waypoints_planner_B.b_gamma <
8239 cartesian_waypoints_planner_B.s_p) {
8240 cartesian_waypoints_planner_B.b_i_h = tmp_0->size[0] *
8241 tmp_0->size[1];
8242 tmp_0->size[0] = 1;
8243 tmp_0->size[1] = grad->size[0];
8244 cartes_emxEnsureCapacity_real_T(tmp_0,
8245 cartesian_waypoints_planner_B.b_i_h);
8246 cartesian_waypoints_planner_B.i_e = grad->size[0];
8247 for (cartesian_waypoints_planner_B.b_i_h = 0;
8248 cartesian_waypoints_planner_B.b_i_h <
8249 cartesian_waypoints_planner_B.i_e;
8250 cartesian_waypoints_planner_B.b_i_h++) {
8251 tmp_0->data[cartesian_waypoints_planner_B.b_i_h] = 0.8 *
8252 grad->data[cartesian_waypoints_planner_B.b_i_h];
8253 }
8254
8255 cartesian_waypoints_planner_B.s_p = 0.0;
8256 for (cartesian_waypoints_planner_B.b_i_h = 0;
8257 cartesian_waypoints_planner_B.b_i_h < 6;
8258 cartesian_waypoints_planner_B.b_i_h++) {
8259 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8260 = 0.0;
8261 for (cartesian_waypoints_planner_B.idx = 0;
8262 cartesian_waypoints_planner_B.idx < 6;
8263 cartesian_waypoints_planner_B.idx++) {
8264 cartesian_waypoints_planner_B.beta =
8265 cartesian_waypoints_planner_B.H[6 *
8266 cartesian_waypoints_planner_B.b_i_h +
8267 cartesian_waypoints_planner_B.idx] * tmp_0->
8268 data[cartesian_waypoints_planner_B.idx] +
8269 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h];
8270 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8271 = cartesian_waypoints_planner_B.beta;
8272 }
8273
8274 cartesian_waypoints_planner_B.s_p +=
8275 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8276 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8277 }
8278
8279 cartesian_waypoints_planner_B.b_i_h = grad_0->size[0] *
8280 grad_0->size[1];
8281 grad_0->size[0] = 1;
8282 grad_0->size[1] = grad->size[0];
8283 cartes_emxEnsureCapacity_real_T(grad_0,
8284 cartesian_waypoints_planner_B.b_i_h);
8285 cartesian_waypoints_planner_B.i_e = grad->size[0];
8286 for (cartesian_waypoints_planner_B.b_i_h = 0;
8287 cartesian_waypoints_planner_B.b_i_h <
8288 cartesian_waypoints_planner_B.i_e;
8289 cartesian_waypoints_planner_B.b_i_h++) {
8290 grad_0->data[cartesian_waypoints_planner_B.b_i_h] = grad->
8291 data[cartesian_waypoints_planner_B.b_i_h];
8292 }
8293
8294 cartesian_waypoints_planner_B.beta = 0.0;
8295 cartesian_waypoints_planner_B.b_gamma = 0.0;
8296 for (cartesian_waypoints_planner_B.b_i_h = 0;
8297 cartesian_waypoints_planner_B.b_i_h < 6;
8298 cartesian_waypoints_planner_B.b_i_h++) {
8299 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8300 = 0.0;
8301 for (cartesian_waypoints_planner_B.idx = 0;
8302 cartesian_waypoints_planner_B.idx < 6;
8303 cartesian_waypoints_planner_B.idx++) {
8304 cartesian_waypoints_planner_B.sigma =
8305 cartesian_waypoints_planner_B.H[6 *
8306 cartesian_waypoints_planner_B.b_i_h +
8307 cartesian_waypoints_planner_B.idx] * grad_0->
8308 data[cartesian_waypoints_planner_B.idx] +
8309 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h];
8310 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8311 = cartesian_waypoints_planner_B.sigma;
8312 }
8313
8314 cartesian_waypoints_planner_B.beta +=
8315 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8316 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8317 cartesian_waypoints_planner_B.b_gamma +=
8318 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]
8319 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8320 }
8321
8322 cartesian_waypoints_planner_B.b_gamma =
8323 cartesian_waypoints_planner_B.s_p /
8324 (cartesian_waypoints_planner_B.beta -
8325 cartesian_waypoints_planner_B.b_gamma);
8326 } else {
8327 cartesian_waypoints_planner_B.b_gamma = 1.0;
8328 }
8329
8330 cartesian_waypoints_planner_B.beta = 0.0;
8331 for (cartesian_waypoints_planner_B.b_i_h = 0;
8332 cartesian_waypoints_planner_B.b_i_h < 6;
8333 cartesian_waypoints_planner_B.b_i_h++) {
8334 cartesian_waypoints_planner_B.s_p = 0.0;
8335 for (cartesian_waypoints_planner_B.idx = 0;
8336 cartesian_waypoints_planner_B.idx < 6;
8337 cartesian_waypoints_planner_B.idx++) {
8338 cartesian_waypoints_planner_B.s_p +=
8339 cartesian_waypoints_planner_B.H[6 *
8340 cartesian_waypoints_planner_B.idx +
8341 cartesian_waypoints_planner_B.b_i_h] * (1.0 -
8342 cartesian_waypoints_planner_B.b_gamma) * grad->
8343 data[cartesian_waypoints_planner_B.idx];
8344 }
8345
8346 cartesian_waypoints_planner_B.s_p +=
8347 cartesian_waypoints_planner_B.b_gamma *
8348 cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];
8349 cartesian_waypoints_planner_B.beta +=
8350 cartesian_waypoints_planner_B.s_p * grad->
8351 data[cartesian_waypoints_planner_B.b_i_h];
8352 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8353 = cartesian_waypoints_planner_B.s_p;
8354 }
8355
8356 cartesian_waypoints_planner_B.b_i_h = sNew->size[0] * sNew->size[1];
8357 sNew->size[0] = 6;
8358 sNew->size[1] = grad->size[0];
8359 cartes_emxEnsureCapacity_real_T(sNew,
8360 cartesian_waypoints_planner_B.b_i_h);
8361 cartesian_waypoints_planner_B.i_e = grad->size[0];
8362 for (cartesian_waypoints_planner_B.b_i_h = 0;
8363 cartesian_waypoints_planner_B.b_i_h <
8364 cartesian_waypoints_planner_B.i_e;
8365 cartesian_waypoints_planner_B.b_i_h++) {
8366 for (cartesian_waypoints_planner_B.idx = 0;
8367 cartesian_waypoints_planner_B.idx < 6;
8368 cartesian_waypoints_planner_B.idx++) {
8369 cartesian_waypoints_planner_B.s_p =
8370 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.idx]
8371 * grad->data[cartesian_waypoints_planner_B.b_i_h];
8372 sNew->data[cartesian_waypoints_planner_B.idx + 6 *
8373 cartesian_waypoints_planner_B.b_i_h] =
8374 cartesian_waypoints_planner_B.s_p /
8375 cartesian_waypoints_planner_B.beta;
8376 }
8377 }
8378
8379 for (cartesian_waypoints_planner_B.b_i_h = 0;
8380 cartesian_waypoints_planner_B.b_i_h < 36;
8381 cartesian_waypoints_planner_B.b_i_h++) {
8382 cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.b_i_h]
8383 =
8384 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
8385 - sNew->data[cartesian_waypoints_planner_B.b_i_h];
8386 }
8387
8388 for (cartesian_waypoints_planner_B.b_i_h = 0;
8389 cartesian_waypoints_planner_B.b_i_h < 6;
8390 cartesian_waypoints_planner_B.b_i_h++) {
8391 for (cartesian_waypoints_planner_B.idx = 0;
8392 cartesian_waypoints_planner_B.idx < 6;
8393 cartesian_waypoints_planner_B.idx++) {
8394 cartesian_waypoints_planner_B.nx_j =
8395 cartesian_waypoints_planner_B.b_i_h + 6 *
8396 cartesian_waypoints_planner_B.idx;
8397 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.nx_j]
8398 = 0.0;
8399 for (cartesian_waypoints_planner_B.i_e = 0;
8400 cartesian_waypoints_planner_B.i_e < 6;
8401 cartesian_waypoints_planner_B.i_e++) {
8402 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.nx_j]
8403 += cartesian_waypoints_planner_B.V[6 *
8404 cartesian_waypoints_planner_B.i_e +
8405 cartesian_waypoints_planner_B.b_i_h] *
8406 cartesian_waypoints_planner_B.H[6 *
8407 cartesian_waypoints_planner_B.idx +
8408 cartesian_waypoints_planner_B.i_e];
8409 }
8410 }
8411 }
8412
8413 for (cartesian_waypoints_planner_B.b_i_h = 0;
8414 cartesian_waypoints_planner_B.b_i_h < 6;
8415 cartesian_waypoints_planner_B.b_i_h++) {
8416 for (cartesian_waypoints_planner_B.idx = 0;
8417 cartesian_waypoints_planner_B.idx < 6;
8418 cartesian_waypoints_planner_B.idx++) {
8419 cartesian_waypoints_planner_B.nx_j =
8420 cartesian_waypoints_planner_B.b_i_h + 6 *
8421 cartesian_waypoints_planner_B.idx;
8422 cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.nx_j]
8423 = 0.0;
8424 for (cartesian_waypoints_planner_B.i_e = 0;
8425 cartesian_waypoints_planner_B.i_e < 6;
8426 cartesian_waypoints_planner_B.i_e++) {
8427 cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.nx_j]
8428 += cartesian_waypoints_planner_B.H_m[6 *
8429 cartesian_waypoints_planner_B.i_e +
8430 cartesian_waypoints_planner_B.b_i_h] *
8431 cartesian_waypoints_planner_B.V[6 *
8432 cartesian_waypoints_planner_B.i_e +
8433 cartesian_waypoints_planner_B.idx];
8434 }
8435 }
8436 }
8437
8438 for (cartesian_waypoints_planner_B.b_i_h = 0;
8439 cartesian_waypoints_planner_B.b_i_h < 6;
8440 cartesian_waypoints_planner_B.b_i_h++) {
8441 for (cartesian_waypoints_planner_B.idx = 0;
8442 cartesian_waypoints_planner_B.idx < 6;
8443 cartesian_waypoints_planner_B.idx++) {
8444 cartesian_waypoints_planner_B.sNew[cartesian_waypoints_planner_B.idx
8445 + 6 * cartesian_waypoints_planner_B.b_i_h] =
8446 cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.idx]
8447 * cartesian_waypoints_planner_B.sNew_p[cartesian_waypoints_planner_B.b_i_h]
8448 / cartesian_waypoints_planner_B.beta;
8449 }
8450 }
8451
8452 for (cartesian_waypoints_planner_B.b_i_h = 0;
8453 cartesian_waypoints_planner_B.b_i_h < 36;
8454 cartesian_waypoints_planner_B.b_i_h++) {
8455 cartesian_waypoints_planner_B.s_p =
8456 cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.b_i_h]
8457 + cartesian_waypoints_planner_B.sNew[cartesian_waypoints_planner_B.b_i_h];
8458 cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.b_i_h]
8459 = 1.4901161193847656E-8 * static_cast<real_T>
8460 (tmp_1[cartesian_waypoints_planner_B.b_i_h]) +
8461 cartesian_waypoints_planner_B.s_p;
8462 cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.b_i_h]
8463 = cartesian_waypoints_planner_B.s_p;
8464 }
8465
8466 if (!cartesian_wa_isPositiveDefinite
8467 (cartesian_waypoints_planner_B.H_m)) {
8468 *exitFlag = HessianNotPositiveSemidefinite;
8469 args = obj->ExtraArgs;
8470 for (cartesian_waypoints_planner_B.b_i_h = 0;
8471 cartesian_waypoints_planner_B.b_i_h < 36;
8472 cartesian_waypoints_planner_B.b_i_h++) {
8473 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
8474 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
8475 }
8476
8477 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
8478 grad->size[0] = args->ErrTemp->size[0];
8479 cartes_emxEnsureCapacity_real_T(grad,
8480 cartesian_waypoints_planner_B.b_i_h);
8481 cartesian_waypoints_planner_B.i_e = args->ErrTemp->size[0];
8482 for (cartesian_waypoints_planner_B.b_i_h = 0;
8483 cartesian_waypoints_planner_B.b_i_h <
8484 cartesian_waypoints_planner_B.i_e;
8485 cartesian_waypoints_planner_B.b_i_h++) {
8486 grad->data[cartesian_waypoints_planner_B.b_i_h] =
8487 args->ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];
8488 }
8489
8490 for (cartesian_waypoints_planner_B.b_i_h = 0;
8491 cartesian_waypoints_planner_B.b_i_h < 6;
8492 cartesian_waypoints_planner_B.b_i_h++) {
8493 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
8494 = 0.0;
8495 for (cartesian_waypoints_planner_B.idx = 0;
8496 cartesian_waypoints_planner_B.idx < 6;
8497 cartesian_waypoints_planner_B.idx++) {
8498 cartesian_waypoints_planner_B.A_d =
8499 cartesian_waypoints_planner_B.unusedU0[6 *
8500 cartesian_waypoints_planner_B.idx +
8501 cartesian_waypoints_planner_B.b_i_h] * grad->
8502 data[cartesian_waypoints_planner_B.idx] +
8503 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
8504 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
8505 = cartesian_waypoints_planner_B.A_d;
8506 }
8507 }
8508
8509 *err = cartesian_waypoints_plan_norm_e
8510 (cartesian_waypoints_planner_B.x);
8511 *iter = static_cast<real_T>
8512 (cartesian_waypoints_planner_B.g_idx_0) + 1.0;
8513 exitg2 = 1;
8514 } else {
8515 guard1 = true;
8516 }
8517 }
8518 }
8519
8520 if (guard1) {
8521 if (DampedBFGSwGradientProjection_e(obj, xSol)) {
8522 for (cartesian_waypoints_planner_B.i_e = 0;
8523 cartesian_waypoints_planner_B.i_e < 6;
8524 cartesian_waypoints_planner_B.i_e++) {
8525 xSol[cartesian_waypoints_planner_B.i_e] =
8526 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_e];
8527 }
8528
8529 *exitFlag = SearchDirectionInvalid;
8530 args = obj->ExtraArgs;
8531 for (cartesian_waypoints_planner_B.b_i_h = 0;
8532 cartesian_waypoints_planner_B.b_i_h < 36;
8533 cartesian_waypoints_planner_B.b_i_h++) {
8534 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
8535 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
8536 }
8537
8538 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
8539 grad->size[0] = args->ErrTemp->size[0];
8540 cartes_emxEnsureCapacity_real_T(grad,
8541 cartesian_waypoints_planner_B.b_i_h);
8542 cartesian_waypoints_planner_B.i_e = args->ErrTemp->size[0];
8543 for (cartesian_waypoints_planner_B.b_i_h = 0;
8544 cartesian_waypoints_planner_B.b_i_h <
8545 cartesian_waypoints_planner_B.i_e;
8546 cartesian_waypoints_planner_B.b_i_h++) {
8547 grad->data[cartesian_waypoints_planner_B.b_i_h] = args->
8548 ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];
8549 }
8550
8551 for (cartesian_waypoints_planner_B.b_i_h = 0;
8552 cartesian_waypoints_planner_B.b_i_h < 6;
8553 cartesian_waypoints_planner_B.b_i_h++) {
8554 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
8555 = 0.0;
8556 for (cartesian_waypoints_planner_B.idx = 0;
8557 cartesian_waypoints_planner_B.idx < 6;
8558 cartesian_waypoints_planner_B.idx++) {
8559 cartesian_waypoints_planner_B.A_d =
8560 cartesian_waypoints_planner_B.unusedU0[6 *
8561 cartesian_waypoints_planner_B.idx +
8562 cartesian_waypoints_planner_B.b_i_h] * grad->
8563 data[cartesian_waypoints_planner_B.idx] +
8564 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
8565 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]
8566 = cartesian_waypoints_planner_B.A_d;
8567 }
8568 }
8569
8570 *err = cartesian_waypoints_plan_norm_e
8571 (cartesian_waypoints_planner_B.x);
8572 *iter = static_cast<real_T>(cartesian_waypoints_planner_B.g_idx_0)
8573 + 1.0;
8574 exitg2 = 1;
8575 } else {
8576 for (cartesian_waypoints_planner_B.i_e = 0;
8577 cartesian_waypoints_planner_B.i_e < 6;
8578 cartesian_waypoints_planner_B.i_e++) {
8579 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_e]
8580 = xSol[cartesian_waypoints_planner_B.i_e];
8581 }
8582
8583 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
8584 grad->size[0] = alpha->size[0];
8585 cartes_emxEnsureCapacity_real_T(grad,
8586 cartesian_waypoints_planner_B.b_i_h);
8587 cartesian_waypoints_planner_B.i_e = alpha->size[0];
8588 for (cartesian_waypoints_planner_B.b_i_h = 0;
8589 cartesian_waypoints_planner_B.b_i_h <
8590 cartesian_waypoints_planner_B.i_e;
8591 cartesian_waypoints_planner_B.b_i_h++) {
8592 grad->data[cartesian_waypoints_planner_B.b_i_h] = alpha->
8593 data[cartesian_waypoints_planner_B.b_i_h];
8594 }
8595
8596 cartesian_waypoints_planner_B.cost =
8597 cartesian_waypoints_planner_B.costNew;
8598 cartesian_waypoints_planner_B.g_idx_0++;
8599 }
8600 }
8601 }
8602 }
8603 } else {
8604 *exitFlag = IterationLimitExceeded;
8605 args = obj->ExtraArgs;
8606 for (cartesian_waypoints_planner_B.b_i_h = 0;
8607 cartesian_waypoints_planner_B.b_i_h < 36;
8608 cartesian_waypoints_planner_B.b_i_h++) {
8609 cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]
8610 = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];
8611 }
8612
8613 cartesian_waypoints_planner_B.b_i_h = grad->size[0];
8614 grad->size[0] = args->ErrTemp->size[0];
8615 cartes_emxEnsureCapacity_real_T(grad, cartesian_waypoints_planner_B.b_i_h);
8616 cartesian_waypoints_planner_B.i_e = args->ErrTemp->size[0];
8617 for (cartesian_waypoints_planner_B.b_i_h = 0;
8618 cartesian_waypoints_planner_B.b_i_h <
8619 cartesian_waypoints_planner_B.i_e;
8620 cartesian_waypoints_planner_B.b_i_h++) {
8621 grad->data[cartesian_waypoints_planner_B.b_i_h] = args->ErrTemp->
8622 data[cartesian_waypoints_planner_B.b_i_h];
8623 }
8624
8625 for (cartesian_waypoints_planner_B.b_i_h = 0;
8626 cartesian_waypoints_planner_B.b_i_h < 6;
8627 cartesian_waypoints_planner_B.b_i_h++) {
8628 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h] =
8629 0.0;
8630 for (cartesian_waypoints_planner_B.idx = 0;
8631 cartesian_waypoints_planner_B.idx < 6;
8632 cartesian_waypoints_planner_B.idx++) {
8633 cartesian_waypoints_planner_B.A_d =
8634 cartesian_waypoints_planner_B.unusedU0[6 *
8635 cartesian_waypoints_planner_B.idx +
8636 cartesian_waypoints_planner_B.b_i_h] * grad->
8637 data[cartesian_waypoints_planner_B.idx] +
8638 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];
8639 cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h] =
8640 cartesian_waypoints_planner_B.A_d;
8641 }
8642 }
8643
8644 *err = cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.x);
8645 *iter = obj->MaxNumIterationInternal;
8646 exitg2 = 1;
8647 }
8648 } while (exitg2 == 0);
8649
8650 cartesian_waypo_emxFree_int32_T(&ii_3);
8651 cartesian_waypoi_emxFree_real_T(&alpha_0);
8652 cartesian_waypoi_emxFree_real_T(&A_3);
8653 cartesian_waypoi_emxFree_real_T(&grad_1);
8654 cartesian_waypo_emxFree_int32_T(&ii_2);
8655 cartesian_waypo_emxFree_int32_T(&ii_1);
8656 cartesian_waypoi_emxFree_real_T(&sNew);
8657 cartesian_waypoi_emxFree_real_T(&grad_0);
8658 cartesian_waypoi_emxFree_real_T(&tmp_0);
8659 cartesian_waypoi_emxFree_real_T(&tmp);
8660 cartesian_waypoi_emxFree_real_T(&sigma);
8661 cartesian_waypoi_emxFree_real_T(&A_2);
8662 cartesian_way_emxFree_boolean_T(&x);
8663 cartesian_waypoi_emxFree_real_T(&y_0);
8664 cartesian_waypo_emxFree_int32_T(&ii_0);
8665 cartesian_waypoi_emxFree_real_T(&y);
8666 cartesian_waypo_emxFree_int32_T(&ii);
8667 cartesian_waypoi_emxFree_real_T(&a);
8668 cartesian_waypo_emxFree_int32_T(&gb);
8669 cartesian_waypo_emxFree_int32_T(&fb);
8670 cartesian_waypo_emxFree_int32_T(&eb);
8671 cartesian_waypo_emxFree_int32_T(&db);
8672 cartesian_waypo_emxFree_int32_T(&cb);
8673 cartesian_waypoi_emxFree_real_T(&L);
8674 cartesian_waypoi_emxFree_real_T(&AIn);
8675 cartesian_waypoi_emxFree_real_T(&alpha);
8676 cartesian_waypoi_emxFree_real_T(&A);
8677 cartesian_way_emxFree_boolean_T(&activeSet);
8678 cartesian_waypoi_emxFree_real_T(&grad);
8679 cartesian_waypoi_emxFree_real_T(&unusedU1);
8680}
8681
8682static void cartesian_waypoints_pl_isfinite(const
8683 emxArray_real_T_cartesian_way_T *x, emxArray_boolean_T_cartesian__T *b)
8684{
8685 emxArray_boolean_T_cartesian__T *tmp;
8686 int32_T loop_ub;
8687 int32_T i;
8688 i = b->size[0];
8689 b->size[0] = x->size[0];
8690 car_emxEnsureCapacity_boolean_T(b, i);
8691 loop_ub = x->size[0];
8692 for (i = 0; i < loop_ub; i++) {
8693 b->data[i] = rtIsInf(x->data[i]);
8694 }
8695
8696 cartesian_way_emxInit_boolean_T(&tmp, 1);
8697 i = tmp->size[0];
8698 tmp->size[0] = x->size[0];
8699 car_emxEnsureCapacity_boolean_T(tmp, i);
8700 loop_ub = x->size[0];
8701 for (i = 0; i < loop_ub; i++) {
8702 tmp->data[i] = rtIsNaN(x->data[i]);
8703 }
8704
8705 i = b->size[0];
8706 car_emxEnsureCapacity_boolean_T(b, i);
8707 loop_ub = b->size[0];
8708 for (i = 0; i < loop_ub; i++) {
8709 b->data[i] = ((!b->data[i]) && (!tmp->data[i]));
8710 }
8711
8712 cartesian_way_emxFree_boolean_T(&tmp);
8713}
8714
8715static void cartesi_genrand_uint32_vector_e(uint32_T mt[625], uint32_T u[2])
8716{
8717 for (cartesian_waypoints_planner_B.b_j_g = 0;
8718 cartesian_waypoints_planner_B.b_j_g < 2;
8719 cartesian_waypoints_planner_B.b_j_g++) {
8720 cartesian_waypoints_planner_B.mti = mt[624] + 1U;
8721 if (cartesian_waypoints_planner_B.mti >= 625U) {
8722 for (cartesian_waypoints_planner_B.b_kk = 0;
8723 cartesian_waypoints_planner_B.b_kk < 227;
8724 cartesian_waypoints_planner_B.b_kk++) {
8725 cartesian_waypoints_planner_B.y_i =
8726 (mt[cartesian_waypoints_planner_B.b_kk + 1] & 2147483647U) |
8727 (mt[cartesian_waypoints_planner_B.b_kk] & 2147483648U);
8728 if ((cartesian_waypoints_planner_B.y_i & 1U) == 0U) {
8729 cartesian_waypoints_planner_B.y_i >>= 1U;
8730 } else {
8731 cartesian_waypoints_planner_B.y_i = cartesian_waypoints_planner_B.y_i >>
8732 1U ^ 2567483615U;
8733 }
8734
8735 mt[cartesian_waypoints_planner_B.b_kk] =
8736 mt[cartesian_waypoints_planner_B.b_kk + 397] ^
8737 cartesian_waypoints_planner_B.y_i;
8738 }
8739
8740 for (cartesian_waypoints_planner_B.b_kk = 0;
8741 cartesian_waypoints_planner_B.b_kk < 396;
8742 cartesian_waypoints_planner_B.b_kk++) {
8743 cartesian_waypoints_planner_B.y_i =
8744 (mt[cartesian_waypoints_planner_B.b_kk + 227] & 2147483648U) |
8745 (mt[cartesian_waypoints_planner_B.b_kk + 228] & 2147483647U);
8746 if ((cartesian_waypoints_planner_B.y_i & 1U) == 0U) {
8747 cartesian_waypoints_planner_B.y_i >>= 1U;
8748 } else {
8749 cartesian_waypoints_planner_B.y_i = cartesian_waypoints_planner_B.y_i >>
8750 1U ^ 2567483615U;
8751 }
8752
8753 mt[cartesian_waypoints_planner_B.b_kk + 227] =
8754 mt[cartesian_waypoints_planner_B.b_kk] ^
8755 cartesian_waypoints_planner_B.y_i;
8756 }
8757
8758 cartesian_waypoints_planner_B.y_i = (mt[623] & 2147483648U) | (mt[0] &
8759 2147483647U);
8760 if ((cartesian_waypoints_planner_B.y_i & 1U) == 0U) {
8761 cartesian_waypoints_planner_B.y_i >>= 1U;
8762 } else {
8763 cartesian_waypoints_planner_B.y_i = cartesian_waypoints_planner_B.y_i >>
8764 1U ^ 2567483615U;
8765 }
8766
8767 mt[623] = mt[396] ^ cartesian_waypoints_planner_B.y_i;
8768 cartesian_waypoints_planner_B.mti = 1U;
8769 }
8770
8771 cartesian_waypoints_planner_B.y_i = mt[static_cast<int32_T>
8772 (cartesian_waypoints_planner_B.mti) - 1];
8773 mt[624] = cartesian_waypoints_planner_B.mti;
8774 cartesian_waypoints_planner_B.y_i ^= cartesian_waypoints_planner_B.y_i >>
8775 11U;
8776 cartesian_waypoints_planner_B.y_i ^= cartesian_waypoints_planner_B.y_i << 7U
8777 & 2636928640U;
8778 cartesian_waypoints_planner_B.y_i ^= cartesian_waypoints_planner_B.y_i <<
8779 15U & 4022730752U;
8780 u[cartesian_waypoints_planner_B.b_j_g] = cartesian_waypoints_planner_B.y_i >>
8781 18U ^ cartesian_waypoints_planner_B.y_i;
8782 }
8783}
8784
8785static boolean_T cartesian_waypoi_is_valid_state(const uint32_T mt[625])
8786{
8787 boolean_T isvalid;
8788 boolean_T exitg1;
8789 if ((mt[624] >= 1U) && (mt[624] < 625U)) {
8790 isvalid = true;
8791 } else {
8792 isvalid = false;
8793 }
8794
8795 if (isvalid) {
8796 isvalid = false;
8797 cartesian_waypoints_planner_B.k_m = 0;
8798 exitg1 = false;
8799 while ((!exitg1) && (cartesian_waypoints_planner_B.k_m + 1 < 625)) {
8800 if (mt[cartesian_waypoints_planner_B.k_m] == 0U) {
8801 cartesian_waypoints_planner_B.k_m++;
8802 } else {
8803 isvalid = true;
8804 exitg1 = true;
8805 }
8806 }
8807 }
8808
8809 return isvalid;
8810}
8811
8812static real_T cartesian_waypoints__genrandu_e(uint32_T mt[625])
8813{
8814 real_T r;
8815 int32_T exitg1;
8816
8817 // ========================= COPYRIGHT NOTICE ============================
8818 // This is a uniform (0,1) pseudorandom number generator based on:
8819 //
8820 // A C-program for MT19937, with initialization improved 2002/1/26.
8821 // Coded by Takuji Nishimura and Makoto Matsumoto.
8822 //
8823 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
8824 // All rights reserved.
8825 //
8826 // Redistribution and use in source and binary forms, with or without
8827 // modification, are permitted provided that the following conditions
8828 // are met:
8829 //
8830 // 1. Redistributions of source code must retain the above copyright
8831 // notice, this list of conditions and the following disclaimer.
8832 //
8833 // 2. Redistributions in binary form must reproduce the above copyright
8834 // notice, this list of conditions and the following disclaimer
8835 // in the documentation and/or other materials provided with the
8836 // distribution.
8837 //
8838 // 3. The names of its contributors may not be used to endorse or
8839 // promote products derived from this software without specific
8840 // prior written permission.
8841 //
8842 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
8843 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
8844 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
8845 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
8846 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
8847 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
8848 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
8849 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
8850 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
8851 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
8852 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
8853 //
8854 // ============================= END =================================
8855 do {
8856 exitg1 = 0;
8857 cartesi_genrand_uint32_vector_e(mt, cartesian_waypoints_planner_B.b_u_b);
8858 r = (static_cast<real_T>(cartesian_waypoints_planner_B.b_u_b[0] >> 5U) *
8859 6.7108864E+7 + static_cast<real_T>(cartesian_waypoints_planner_B.b_u_b
8860 [1] >> 6U)) * 1.1102230246251565E-16;
8861 if (r == 0.0) {
8862 if (!cartesian_waypoi_is_valid_state(mt)) {
8863 cartesian_waypoints_planner_B.r_k = 5489U;
8864 mt[0] = 5489U;
8865 for (cartesian_waypoints_planner_B.b_mti_e = 0;
8866 cartesian_waypoints_planner_B.b_mti_e < 623;
8867 cartesian_waypoints_planner_B.b_mti_e++) {
8868 cartesian_waypoints_planner_B.r_k =
8869 ((cartesian_waypoints_planner_B.r_k >> 30U ^
8870 cartesian_waypoints_planner_B.r_k) * 1812433253U +
8871 cartesian_waypoints_planner_B.b_mti_e) + 1U;
8872 mt[cartesian_waypoints_planner_B.b_mti_e + 1] =
8873 cartesian_waypoints_planner_B.r_k;
8874 }
8875
8876 mt[624] = 624U;
8877 }
8878 } else {
8879 exitg1 = 1;
8880 }
8881 } while (exitg1 == 0);
8882
8883 return r;
8884}
8885
8886static real_T cartesia_eml_rand_mt19937ar_evq(uint32_T state[625])
8887{
8888 real_T r;
8889 static const real_T tmp[257] = { 1.0, 0.977101701267673, 0.959879091800108,
8890 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
8891 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
8892 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
8893 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
8894 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
8895 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
8896 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
8897 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
8898 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
8899 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
8900 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
8901 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
8902 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
8903 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
8904 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
8905 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
8906 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
8907 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
8908 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
8909 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
8910 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
8911 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
8912 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
8913 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
8914 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
8915 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
8916 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
8917 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
8918 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
8919 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
8920 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
8921 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
8922 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
8923 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
8924 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
8925 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
8926 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
8927 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
8928 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
8929 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
8930 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
8931 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
8932 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
8933 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
8934 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
8935 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
8936 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
8937 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
8938 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
8939 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
8940 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
8941 0.093365311912691, 0.0911136480663738, 0.0888735920682759,
8942 0.0866451944505581, 0.0844285095703535, 0.082223595813203,
8943 0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
8944 0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
8945 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
8946 0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
8947 0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
8948 0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
8949 0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
8950 0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
8951 0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
8952 0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
8953 0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
8954 0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
8955 0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
8956 0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
8957 0.000477467764609386 };
8958
8959 const real_T *fitab;
8960 int32_T exitg1;
8961 cartesian_waypoints_planner_B.xi[0] = 0.0;
8962 cartesian_waypoints_planner_B.xi[1] = 0.215241895984875;
8963 cartesian_waypoints_planner_B.xi[2] = 0.286174591792068;
8964 cartesian_waypoints_planner_B.xi[3] = 0.335737519214422;
8965 cartesian_waypoints_planner_B.xi[4] = 0.375121332878378;
8966 cartesian_waypoints_planner_B.xi[5] = 0.408389134611989;
8967 cartesian_waypoints_planner_B.xi[6] = 0.43751840220787;
8968 cartesian_waypoints_planner_B.xi[7] = 0.46363433679088;
8969 cartesian_waypoints_planner_B.xi[8] = 0.487443966139235;
8970 cartesian_waypoints_planner_B.xi[9] = 0.50942332960209;
8971 cartesian_waypoints_planner_B.xi[10] = 0.529909720661557;
8972 cartesian_waypoints_planner_B.xi[11] = 0.549151702327164;
8973 cartesian_waypoints_planner_B.xi[12] = 0.567338257053817;
8974 cartesian_waypoints_planner_B.xi[13] = 0.584616766106378;
8975 cartesian_waypoints_planner_B.xi[14] = 0.601104617755991;
8976 cartesian_waypoints_planner_B.xi[15] = 0.61689699000775;
8977 cartesian_waypoints_planner_B.xi[16] = 0.63207223638606;
8978 cartesian_waypoints_planner_B.xi[17] = 0.646695714894993;
8979 cartesian_waypoints_planner_B.xi[18] = 0.660822574244419;
8980 cartesian_waypoints_planner_B.xi[19] = 0.674499822837293;
8981 cartesian_waypoints_planner_B.xi[20] = 0.687767892795788;
8982 cartesian_waypoints_planner_B.xi[21] = 0.700661841106814;
8983 cartesian_waypoints_planner_B.xi[22] = 0.713212285190975;
8984 cartesian_waypoints_planner_B.xi[23] = 0.725446140909999;
8985 cartesian_waypoints_planner_B.xi[24] = 0.737387211434295;
8986 cartesian_waypoints_planner_B.xi[25] = 0.749056662017815;
8987 cartesian_waypoints_planner_B.xi[26] = 0.760473406430107;
8988 cartesian_waypoints_planner_B.xi[27] = 0.771654424224568;
8989 cartesian_waypoints_planner_B.xi[28] = 0.782615023307232;
8990 cartesian_waypoints_planner_B.xi[29] = 0.793369058840623;
8991 cartesian_waypoints_planner_B.xi[30] = 0.80392911698997;
8992 cartesian_waypoints_planner_B.xi[31] = 0.814306670135215;
8993 cartesian_waypoints_planner_B.xi[32] = 0.824512208752291;
8994 cartesian_waypoints_planner_B.xi[33] = 0.834555354086381;
8995 cartesian_waypoints_planner_B.xi[34] = 0.844444954909153;
8996 cartesian_waypoints_planner_B.xi[35] = 0.854189171008163;
8997 cartesian_waypoints_planner_B.xi[36] = 0.863795545553308;
8998 cartesian_waypoints_planner_B.xi[37] = 0.87327106808886;
8999 cartesian_waypoints_planner_B.xi[38] = 0.882622229585165;
9000 cartesian_waypoints_planner_B.xi[39] = 0.891855070732941;
9001 cartesian_waypoints_planner_B.xi[40] = 0.900975224461221;
9002 cartesian_waypoints_planner_B.xi[41] = 0.909987953496718;
9003 cartesian_waypoints_planner_B.xi[42] = 0.91889818364959;
9004 cartesian_waypoints_planner_B.xi[43] = 0.927710533401999;
9005 cartesian_waypoints_planner_B.xi[44] = 0.936429340286575;
9006 cartesian_waypoints_planner_B.xi[45] = 0.945058684468165;
9007 cartesian_waypoints_planner_B.xi[46] = 0.953602409881086;
9008 cartesian_waypoints_planner_B.xi[47] = 0.96206414322304;
9009 cartesian_waypoints_planner_B.xi[48] = 0.970447311064224;
9010 cartesian_waypoints_planner_B.xi[49] = 0.978755155294224;
9011 cartesian_waypoints_planner_B.xi[50] = 0.986990747099062;
9012 cartesian_waypoints_planner_B.xi[51] = 0.99515699963509;
9013 cartesian_waypoints_planner_B.xi[52] = 1.00325667954467;
9014 cartesian_waypoints_planner_B.xi[53] = 1.01129241744;
9015 cartesian_waypoints_planner_B.xi[54] = 1.01926671746548;
9016 cartesian_waypoints_planner_B.xi[55] = 1.02718196603564;
9017 cartesian_waypoints_planner_B.xi[56] = 1.03504043983344;
9018 cartesian_waypoints_planner_B.xi[57] = 1.04284431314415;
9019 cartesian_waypoints_planner_B.xi[58] = 1.05059566459093;
9020 cartesian_waypoints_planner_B.xi[59] = 1.05829648333067;
9021 cartesian_waypoints_planner_B.xi[60] = 1.06594867476212;
9022 cartesian_waypoints_planner_B.xi[61] = 1.07355406579244;
9023 cartesian_waypoints_planner_B.xi[62] = 1.0811144097034;
9024 cartesian_waypoints_planner_B.xi[63] = 1.08863139065398;
9025 cartesian_waypoints_planner_B.xi[64] = 1.09610662785202;
9026 cartesian_waypoints_planner_B.xi[65] = 1.10354167942464;
9027 cartesian_waypoints_planner_B.xi[66] = 1.11093804601357;
9028 cartesian_waypoints_planner_B.xi[67] = 1.11829717411934;
9029 cartesian_waypoints_planner_B.xi[68] = 1.12562045921553;
9030 cartesian_waypoints_planner_B.xi[69] = 1.13290924865253;
9031 cartesian_waypoints_planner_B.xi[70] = 1.14016484436815;
9032 cartesian_waypoints_planner_B.xi[71] = 1.14738850542085;
9033 cartesian_waypoints_planner_B.xi[72] = 1.15458145035993;
9034 cartesian_waypoints_planner_B.xi[73] = 1.16174485944561;
9035 cartesian_waypoints_planner_B.xi[74] = 1.16887987673083;
9036 cartesian_waypoints_planner_B.xi[75] = 1.17598761201545;
9037 cartesian_waypoints_planner_B.xi[76] = 1.18306914268269;
9038 cartesian_waypoints_planner_B.xi[77] = 1.19012551542669;
9039 cartesian_waypoints_planner_B.xi[78] = 1.19715774787944;
9040 cartesian_waypoints_planner_B.xi[79] = 1.20416683014438;
9041 cartesian_waypoints_planner_B.xi[80] = 1.2111537262437;
9042 cartesian_waypoints_planner_B.xi[81] = 1.21811937548548;
9043 cartesian_waypoints_planner_B.xi[82] = 1.22506469375653;
9044 cartesian_waypoints_planner_B.xi[83] = 1.23199057474614;
9045 cartesian_waypoints_planner_B.xi[84] = 1.23889789110569;
9046 cartesian_waypoints_planner_B.xi[85] = 1.24578749554863;
9047 cartesian_waypoints_planner_B.xi[86] = 1.2526602218949;
9048 cartesian_waypoints_planner_B.xi[87] = 1.25951688606371;
9049 cartesian_waypoints_planner_B.xi[88] = 1.26635828701823;
9050 cartesian_waypoints_planner_B.xi[89] = 1.27318520766536;
9051 cartesian_waypoints_planner_B.xi[90] = 1.27999841571382;
9052 cartesian_waypoints_planner_B.xi[91] = 1.28679866449324;
9053 cartesian_waypoints_planner_B.xi[92] = 1.29358669373695;
9054 cartesian_waypoints_planner_B.xi[93] = 1.30036323033084;
9055 cartesian_waypoints_planner_B.xi[94] = 1.30712898903073;
9056 cartesian_waypoints_planner_B.xi[95] = 1.31388467315022;
9057 cartesian_waypoints_planner_B.xi[96] = 1.32063097522106;
9058 cartesian_waypoints_planner_B.xi[97] = 1.32736857762793;
9059 cartesian_waypoints_planner_B.xi[98] = 1.33409815321936;
9060 cartesian_waypoints_planner_B.xi[99] = 1.3408203658964;
9061 cartesian_waypoints_planner_B.xi[100] = 1.34753587118059;
9062 cartesian_waypoints_planner_B.xi[101] = 1.35424531676263;
9063 cartesian_waypoints_planner_B.xi[102] = 1.36094934303328;
9064 cartesian_waypoints_planner_B.xi[103] = 1.36764858359748;
9065 cartesian_waypoints_planner_B.xi[104] = 1.37434366577317;
9066 cartesian_waypoints_planner_B.xi[105] = 1.38103521107586;
9067 cartesian_waypoints_planner_B.xi[106] = 1.38772383568998;
9068 cartesian_waypoints_planner_B.xi[107] = 1.39441015092814;
9069 cartesian_waypoints_planner_B.xi[108] = 1.40109476367925;
9070 cartesian_waypoints_planner_B.xi[109] = 1.4077782768464;
9071 cartesian_waypoints_planner_B.xi[110] = 1.41446128977547;
9072 cartesian_waypoints_planner_B.xi[111] = 1.42114439867531;
9073 cartesian_waypoints_planner_B.xi[112] = 1.42782819703026;
9074 cartesian_waypoints_planner_B.xi[113] = 1.43451327600589;
9075 cartesian_waypoints_planner_B.xi[114] = 1.44120022484872;
9076 cartesian_waypoints_planner_B.xi[115] = 1.44788963128058;
9077 cartesian_waypoints_planner_B.xi[116] = 1.45458208188841;
9078 cartesian_waypoints_planner_B.xi[117] = 1.46127816251028;
9079 cartesian_waypoints_planner_B.xi[118] = 1.46797845861808;
9080 cartesian_waypoints_planner_B.xi[119] = 1.47468355569786;
9081 cartesian_waypoints_planner_B.xi[120] = 1.48139403962819;
9082 cartesian_waypoints_planner_B.xi[121] = 1.48811049705745;
9083 cartesian_waypoints_planner_B.xi[122] = 1.49483351578049;
9084 cartesian_waypoints_planner_B.xi[123] = 1.50156368511546;
9085 cartesian_waypoints_planner_B.xi[124] = 1.50830159628131;
9086 cartesian_waypoints_planner_B.xi[125] = 1.51504784277671;
9087 cartesian_waypoints_planner_B.xi[126] = 1.521803020761;
9088 cartesian_waypoints_planner_B.xi[127] = 1.52856772943771;
9089 cartesian_waypoints_planner_B.xi[128] = 1.53534257144151;
9090 cartesian_waypoints_planner_B.xi[129] = 1.542128153229;
9091 cartesian_waypoints_planner_B.xi[130] = 1.54892508547417;
9092 cartesian_waypoints_planner_B.xi[131] = 1.55573398346918;
9093 cartesian_waypoints_planner_B.xi[132] = 1.56255546753104;
9094 cartesian_waypoints_planner_B.xi[133] = 1.56939016341512;
9095 cartesian_waypoints_planner_B.xi[134] = 1.57623870273591;
9096 cartesian_waypoints_planner_B.xi[135] = 1.58310172339603;
9097 cartesian_waypoints_planner_B.xi[136] = 1.58997987002419;
9098 cartesian_waypoints_planner_B.xi[137] = 1.59687379442279;
9099 cartesian_waypoints_planner_B.xi[138] = 1.60378415602609;
9100 cartesian_waypoints_planner_B.xi[139] = 1.61071162236983;
9101 cartesian_waypoints_planner_B.xi[140] = 1.61765686957301;
9102 cartesian_waypoints_planner_B.xi[141] = 1.62462058283303;
9103 cartesian_waypoints_planner_B.xi[142] = 1.63160345693487;
9104 cartesian_waypoints_planner_B.xi[143] = 1.63860619677555;
9105 cartesian_waypoints_planner_B.xi[144] = 1.64562951790478;
9106 cartesian_waypoints_planner_B.xi[145] = 1.65267414708306;
9107 cartesian_waypoints_planner_B.xi[146] = 1.65974082285818;
9108 cartesian_waypoints_planner_B.xi[147] = 1.66683029616166;
9109 cartesian_waypoints_planner_B.xi[148] = 1.67394333092612;
9110 cartesian_waypoints_planner_B.xi[149] = 1.68108070472517;
9111 cartesian_waypoints_planner_B.xi[150] = 1.68824320943719;
9112 cartesian_waypoints_planner_B.xi[151] = 1.69543165193456;
9113 cartesian_waypoints_planner_B.xi[152] = 1.70264685479992;
9114 cartesian_waypoints_planner_B.xi[153] = 1.7098896570713;
9115 cartesian_waypoints_planner_B.xi[154] = 1.71716091501782;
9116 cartesian_waypoints_planner_B.xi[155] = 1.72446150294804;
9117 cartesian_waypoints_planner_B.xi[156] = 1.73179231405296;
9118 cartesian_waypoints_planner_B.xi[157] = 1.73915426128591;
9119 cartesian_waypoints_planner_B.xi[158] = 1.74654827828172;
9120 cartesian_waypoints_planner_B.xi[159] = 1.75397532031767;
9121 cartesian_waypoints_planner_B.xi[160] = 1.76143636531891;
9122 cartesian_waypoints_planner_B.xi[161] = 1.76893241491127;
9123 cartesian_waypoints_planner_B.xi[162] = 1.77646449552452;
9124 cartesian_waypoints_planner_B.xi[163] = 1.78403365954944;
9125 cartesian_waypoints_planner_B.xi[164] = 1.79164098655216;
9126 cartesian_waypoints_planner_B.xi[165] = 1.79928758454972;
9127 cartesian_waypoints_planner_B.xi[166] = 1.80697459135082;
9128 cartesian_waypoints_planner_B.xi[167] = 1.81470317596628;
9129 cartesian_waypoints_planner_B.xi[168] = 1.82247454009388;
9130 cartesian_waypoints_planner_B.xi[169] = 1.83028991968276;
9131 cartesian_waypoints_planner_B.xi[170] = 1.83815058658281;
9132 cartesian_waypoints_planner_B.xi[171] = 1.84605785028518;
9133 cartesian_waypoints_planner_B.xi[172] = 1.8540130597602;
9134 cartesian_waypoints_planner_B.xi[173] = 1.86201760539967;
9135 cartesian_waypoints_planner_B.xi[174] = 1.87007292107127;
9136 cartesian_waypoints_planner_B.xi[175] = 1.878180486293;
9137 cartesian_waypoints_planner_B.xi[176] = 1.88634182853678;
9138 cartesian_waypoints_planner_B.xi[177] = 1.8945585256707;
9139 cartesian_waypoints_planner_B.xi[178] = 1.90283220855043;
9140 cartesian_waypoints_planner_B.xi[179] = 1.91116456377125;
9141 cartesian_waypoints_planner_B.xi[180] = 1.91955733659319;
9142 cartesian_waypoints_planner_B.xi[181] = 1.92801233405266;
9143 cartesian_waypoints_planner_B.xi[182] = 1.93653142827569;
9144 cartesian_waypoints_planner_B.xi[183] = 1.94511656000868;
9145 cartesian_waypoints_planner_B.xi[184] = 1.95376974238465;
9146 cartesian_waypoints_planner_B.xi[185] = 1.96249306494436;
9147 cartesian_waypoints_planner_B.xi[186] = 1.97128869793366;
9148 cartesian_waypoints_planner_B.xi[187] = 1.98015889690048;
9149 cartesian_waypoints_planner_B.xi[188] = 1.98910600761744;
9150 cartesian_waypoints_planner_B.xi[189] = 1.99813247135842;
9151 cartesian_waypoints_planner_B.xi[190] = 2.00724083056053;
9152 cartesian_waypoints_planner_B.xi[191] = 2.0164337349062;
9153 cartesian_waypoints_planner_B.xi[192] = 2.02571394786385;
9154 cartesian_waypoints_planner_B.xi[193] = 2.03508435372962;
9155 cartesian_waypoints_planner_B.xi[194] = 2.04454796521753;
9156 cartesian_waypoints_planner_B.xi[195] = 2.05410793165065;
9157 cartesian_waypoints_planner_B.xi[196] = 2.06376754781173;
9158 cartesian_waypoints_planner_B.xi[197] = 2.07353026351874;
9159 cartesian_waypoints_planner_B.xi[198] = 2.0833996939983;
9160 cartesian_waypoints_planner_B.xi[199] = 2.09337963113879;
9161 cartesian_waypoints_planner_B.xi[200] = 2.10347405571488;
9162 cartesian_waypoints_planner_B.xi[201] = 2.11368715068665;
9163 cartesian_waypoints_planner_B.xi[202] = 2.12402331568952;
9164 cartesian_waypoints_planner_B.xi[203] = 2.13448718284602;
9165 cartesian_waypoints_planner_B.xi[204] = 2.14508363404789;
9166 cartesian_waypoints_planner_B.xi[205] = 2.15581781987674;
9167 cartesian_waypoints_planner_B.xi[206] = 2.16669518035431;
9168 cartesian_waypoints_planner_B.xi[207] = 2.17772146774029;
9169 cartesian_waypoints_planner_B.xi[208] = 2.18890277162636;
9170 cartesian_waypoints_planner_B.xi[209] = 2.20024554661128;
9171 cartesian_waypoints_planner_B.xi[210] = 2.21175664288416;
9172 cartesian_waypoints_planner_B.xi[211] = 2.22344334009251;
9173 cartesian_waypoints_planner_B.xi[212] = 2.23531338492992;
9174 cartesian_waypoints_planner_B.xi[213] = 2.24737503294739;
9175 cartesian_waypoints_planner_B.xi[214] = 2.25963709517379;
9176 cartesian_waypoints_planner_B.xi[215] = 2.27210899022838;
9177 cartesian_waypoints_planner_B.xi[216] = 2.28480080272449;
9178 cartesian_waypoints_planner_B.xi[217] = 2.29772334890286;
9179 cartesian_waypoints_planner_B.xi[218] = 2.31088825060137;
9180 cartesian_waypoints_planner_B.xi[219] = 2.32430801887113;
9181 cartesian_waypoints_planner_B.xi[220] = 2.33799614879653;
9182 cartesian_waypoints_planner_B.xi[221] = 2.35196722737914;
9183 cartesian_waypoints_planner_B.xi[222] = 2.36623705671729;
9184 cartesian_waypoints_planner_B.xi[223] = 2.38082279517208;
9185 cartesian_waypoints_planner_B.xi[224] = 2.39574311978193;
9186 cartesian_waypoints_planner_B.xi[225] = 2.41101841390112;
9187 cartesian_waypoints_planner_B.xi[226] = 2.42667098493715;
9188 cartesian_waypoints_planner_B.xi[227] = 2.44272531820036;
9189 cartesian_waypoints_planner_B.xi[228] = 2.4592083743347;
9190 cartesian_waypoints_planner_B.xi[229] = 2.47614993967052;
9191 cartesian_waypoints_planner_B.xi[230] = 2.49358304127105;
9192 cartesian_waypoints_planner_B.xi[231] = 2.51154444162669;
9193 cartesian_waypoints_planner_B.xi[232] = 2.53007523215985;
9194 cartesian_waypoints_planner_B.xi[233] = 2.54922155032478;
9195 cartesian_waypoints_planner_B.xi[234] = 2.56903545268184;
9196 cartesian_waypoints_planner_B.xi[235] = 2.58957598670829;
9197 cartesian_waypoints_planner_B.xi[236] = 2.61091051848882;
9198 cartesian_waypoints_planner_B.xi[237] = 2.63311639363158;
9199 cartesian_waypoints_planner_B.xi[238] = 2.65628303757674;
9200 cartesian_waypoints_planner_B.xi[239] = 2.68051464328574;
9201 cartesian_waypoints_planner_B.xi[240] = 2.70593365612306;
9202 cartesian_waypoints_planner_B.xi[241] = 2.73268535904401;
9203 cartesian_waypoints_planner_B.xi[242] = 2.76094400527999;
9204 cartesian_waypoints_planner_B.xi[243] = 2.79092117400193;
9205 cartesian_waypoints_planner_B.xi[244] = 2.82287739682644;
9206 cartesian_waypoints_planner_B.xi[245] = 2.85713873087322;
9207 cartesian_waypoints_planner_B.xi[246] = 2.89412105361341;
9208 cartesian_waypoints_planner_B.xi[247] = 2.93436686720889;
9209 cartesian_waypoints_planner_B.xi[248] = 2.97860327988184;
9210 cartesian_waypoints_planner_B.xi[249] = 3.02783779176959;
9211 cartesian_waypoints_planner_B.xi[250] = 3.08352613200214;
9212 cartesian_waypoints_planner_B.xi[251] = 3.147889289518;
9213 cartesian_waypoints_planner_B.xi[252] = 3.2245750520478;
9214 cartesian_waypoints_planner_B.xi[253] = 3.32024473383983;
9215 cartesian_waypoints_planner_B.xi[254] = 3.44927829856143;
9216 cartesian_waypoints_planner_B.xi[255] = 3.65415288536101;
9217 cartesian_waypoints_planner_B.xi[256] = 3.91075795952492;
9218 fitab = &tmp[0];
9219 do {
9220 exitg1 = 0;
9221 cartesi_genrand_uint32_vector_e(state, cartesian_waypoints_planner_B.u32);
9222 cartesian_waypoints_planner_B.i_c = static_cast<int32_T>
9223 ((cartesian_waypoints_planner_B.u32[1] >> 24U) + 1U);
9224 r = ((static_cast<real_T>(cartesian_waypoints_planner_B.u32[0] >> 3U) *
9225 1.6777216E+7 + static_cast<real_T>(static_cast<int32_T>
9226 (cartesian_waypoints_planner_B.u32[1]) & 16777215)) *
9227 2.2204460492503131E-16 - 1.0) *
9228 cartesian_waypoints_planner_B.xi[cartesian_waypoints_planner_B.i_c];
9229 if (fabs(r) <=
9230 cartesian_waypoints_planner_B.xi[cartesian_waypoints_planner_B.i_c - 1])
9231 {
9232 exitg1 = 1;
9233 } else if (cartesian_waypoints_planner_B.i_c < 256) {
9234 cartesian_waypoints_planner_B.x_d = cartesian_waypoints__genrandu_e(state);
9235 if ((fitab[cartesian_waypoints_planner_B.i_c - 1] -
9236 fitab[cartesian_waypoints_planner_B.i_c]) *
9237 cartesian_waypoints_planner_B.x_d +
9238 fitab[cartesian_waypoints_planner_B.i_c] < exp(-0.5 * r * r)) {
9239 exitg1 = 1;
9240 }
9241 } else {
9242 do {
9243 cartesian_waypoints_planner_B.x_d = cartesian_waypoints__genrandu_e
9244 (state);
9245 cartesian_waypoints_planner_B.x_d = log
9246 (cartesian_waypoints_planner_B.x_d) * 0.273661237329758;
9247 cartesian_waypoints_planner_B.d_u = cartesian_waypoints__genrandu_e
9248 (state);
9249 } while (!(-2.0 * log(cartesian_waypoints_planner_B.d_u) >
9250 cartesian_waypoints_planner_B.x_d *
9251 cartesian_waypoints_planner_B.x_d));
9252
9253 if (r < 0.0) {
9254 r = cartesian_waypoints_planner_B.x_d - 3.65415288536101;
9255 } else {
9256 r = 3.65415288536101 - cartesian_waypoints_planner_B.x_d;
9257 }
9258
9259 exitg1 = 1;
9260 }
9261 } while (exitg1 == 0);
9262
9263 return r;
9264}
9265
9266static void cartesian_waypoints_plann_randn(const real_T varargin_1[2],
9267 emxArray_real_T_cartesian_way_T *r)
9268{
9269 cartesian_waypoints_planner_B.b_k_j = r->size[0] * r->size[1];
9270 r->size[0] = static_cast<int32_T>(varargin_1[0]);
9271 r->size[1] = 1;
9272 cartes_emxEnsureCapacity_real_T(r, cartesian_waypoints_planner_B.b_k_j);
9273 cartesian_waypoints_planner_B.d_g = r->size[0] - 1;
9274 for (cartesian_waypoints_planner_B.b_k_j = 0;
9275 cartesian_waypoints_planner_B.b_k_j <= cartesian_waypoints_planner_B.d_g;
9276 cartesian_waypoints_planner_B.b_k_j++) {
9277 r->data[cartesian_waypoints_planner_B.b_k_j] =
9278 cartesia_eml_rand_mt19937ar_evq(cartesian_waypoints_planner_DW.state_m);
9279 }
9280}
9281
9282static void cartesian__eml_rand_mt19937ar_e(const uint32_T state[625], uint32_T
9283 b_state[625], real_T *r)
9284{
9285 int32_T exitg1;
9286 memcpy(&b_state[0], &state[0], 625U * sizeof(uint32_T));
9287
9288 // ========================= COPYRIGHT NOTICE ============================
9289 // This is a uniform (0,1) pseudorandom number generator based on:
9290 //
9291 // A C-program for MT19937, with initialization improved 2002/1/26.
9292 // Coded by Takuji Nishimura and Makoto Matsumoto.
9293 //
9294 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
9295 // All rights reserved.
9296 //
9297 // Redistribution and use in source and binary forms, with or without
9298 // modification, are permitted provided that the following conditions
9299 // are met:
9300 //
9301 // 1. Redistributions of source code must retain the above copyright
9302 // notice, this list of conditions and the following disclaimer.
9303 //
9304 // 2. Redistributions in binary form must reproduce the above copyright
9305 // notice, this list of conditions and the following disclaimer
9306 // in the documentation and/or other materials provided with the
9307 // distribution.
9308 //
9309 // 3. The names of its contributors may not be used to endorse or
9310 // promote products derived from this software without specific
9311 // prior written permission.
9312 //
9313 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
9314 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
9315 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
9316 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
9317 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
9318 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
9319 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
9320 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
9321 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
9322 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
9323 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
9324 //
9325 // ============================= END =================================
9326 do {
9327 exitg1 = 0;
9328 cartesi_genrand_uint32_vector_e(b_state, cartesian_waypoints_planner_B.b_u);
9329 *r = (static_cast<real_T>(cartesian_waypoints_planner_B.b_u[0] >> 5U) *
9330 6.7108864E+7 + static_cast<real_T>(cartesian_waypoints_planner_B.b_u[1]
9331 >> 6U)) * 1.1102230246251565E-16;
9332 if (*r == 0.0) {
9333 if (!cartesian_waypoi_is_valid_state(b_state)) {
9334 cartesian_waypoints_planner_B.r = 5489U;
9335 b_state[0] = 5489U;
9336 for (cartesian_waypoints_planner_B.b_mti = 0;
9337 cartesian_waypoints_planner_B.b_mti < 623;
9338 cartesian_waypoints_planner_B.b_mti++) {
9339 cartesian_waypoints_planner_B.r = ((cartesian_waypoints_planner_B.r >>
9340 30U ^ cartesian_waypoints_planner_B.r) * 1812433253U +
9341 cartesian_waypoints_planner_B.b_mti) + 1U;
9342 b_state[cartesian_waypoints_planner_B.b_mti + 1] =
9343 cartesian_waypoints_planner_B.r;
9344 }
9345
9346 b_state[624] = 624U;
9347 }
9348 } else {
9349 exitg1 = 1;
9350 }
9351 } while (exitg1 == 0);
9352}
9353
9354static void cartesian_waypoints_plan_rand_e(real_T varargin_1,
9355 emxArray_real_T_cartesian_way_T *r)
9356{
9357 cartesian_waypoints_planner_B.b_k_d = r->size[0];
9358 cartesian_waypoints_planner_B.d = static_cast<int32_T>(varargin_1);
9359 r->size[0] = cartesian_waypoints_planner_B.d;
9360 cartes_emxEnsureCapacity_real_T(r, cartesian_waypoints_planner_B.b_k_d);
9361 cartesian_waypoints_planner_B.d--;
9362 for (cartesian_waypoints_planner_B.b_k_d = 0;
9363 cartesian_waypoints_planner_B.b_k_d <= cartesian_waypoints_planner_B.d;
9364 cartesian_waypoints_planner_B.b_k_d++) {
9365 memcpy(&cartesian_waypoints_planner_B.uv1[0],
9366 &cartesian_waypoints_planner_DW.state_m[0], 625U * sizeof(uint32_T));
9367 cartesian__eml_rand_mt19937ar_e(cartesian_waypoints_planner_B.uv1,
9368 cartesian_waypoints_planner_DW.state_m, &r->
9369 data[cartesian_waypoints_planner_B.b_k_d]);
9370 }
9371}
9372
9373static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
9374 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
9375 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
9376 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
9377 solutionInfo_Status_size[2])
9378{
9379 emxArray_real_T_cartesian_way_T *newseed;
9380 f_robotics_manip_internal_IKE_T *args;
9381 x_robotics_manip_internal_Rig_T *obj_0;
9382 emxArray_real_T_cartesian_way_T *qi;
9383 c_rigidBodyJoint_cartesian__e_T *obj_1;
9384 emxArray_real_T_cartesian_way_T *ub;
9385 emxArray_real_T_cartesian_way_T *lb;
9386 emxArray_real_T_cartesian_way_T *rn;
9387 emxArray_real_T_cartesian_way_T *e;
9388 emxArray_boolean_T_cartesian__T *x;
9389 emxArray_boolean_T_cartesian__T *x_tmp;
9390 emxArray_boolean_T_cartesian__T *x_tmp_0;
9391 emxArray_boolean_T_cartesian__T *x_0;
9392 static const char_T tmp[14] = { 'b', 'e', 's', 't', ' ', 'a', 'v', 'a', 'i',
9393 'l', 'a', 'b', 'l', 'e' };
9394
9395 static const char_T tmp_0[7] = { 's', 'u', 'c', 'c', 'e', 's', 's' };
9396
9397 boolean_T guard1 = false;
9398 boolean_T guard2 = false;
9399 boolean_T guard3 = false;
9400 boolean_T exitg1;
9401 boolean_T exitg2;
9402 obj->MaxNumIterationInternal = obj->MaxNumIteration;
9403 obj->MaxTimeInternal = obj->MaxTime;
9404 for (cartesian_waypoints_planner_B.i_f = 0; cartesian_waypoints_planner_B.i_f <
9405 6; cartesian_waypoints_planner_B.i_f++) {
9406 obj->SeedInternal[cartesian_waypoints_planner_B.i_f] =
9407 seed[cartesian_waypoints_planner_B.i_f];
9408 }
9409
9410 cartesian_waypoints_planner_B.tol = obj->SolutionTolerance;
9411 obj->TimeObj.StartTime = ctimefun();
9412 DampedBFGSwGradientProjection_s(obj, xSol,
9413 &cartesian_waypoints_planner_B.exitFlag, &cartesian_waypoints_planner_B.err,
9414 &cartesian_waypoints_planner_B.iter);
9415 *solutionInfo_RRAttempts = 0.0;
9416 *solutionInfo_Iterations = cartesian_waypoints_planner_B.iter;
9417 *solutionInfo_Error = cartesian_waypoints_planner_B.err;
9418 cartesian_waypoints_planner_B.exitFlagPrev =
9419 cartesian_waypoints_planner_B.exitFlag;
9420 cartesian_waypoi_emxInit_real_T(&newseed, 1);
9421 cartesian_waypoi_emxInit_real_T(&qi, 2);
9422 cartesian_waypoi_emxInit_real_T(&ub, 1);
9423 cartesian_waypoi_emxInit_real_T(&lb, 1);
9424 cartesian_waypoi_emxInit_real_T(&rn, 1);
9425 cartesian_waypoi_emxInit_real_T(&e, 2);
9426 cartesian_way_emxInit_boolean_T(&x, 1);
9427 cartesian_way_emxInit_boolean_T(&x_tmp, 1);
9428 cartesian_way_emxInit_boolean_T(&x_tmp_0, 1);
9429 cartesian_way_emxInit_boolean_T(&x_0, 1);
9430 exitg1 = false;
9431 while ((!exitg1) && (obj->RandomRestart && (cartesian_waypoints_planner_B.err >
9432 cartesian_waypoints_planner_B.tol))) {
9433 obj->MaxNumIterationInternal -= cartesian_waypoints_planner_B.iter;
9434 cartesian_waypoints_planner_B.err = ctimefun();
9435 cartesian_waypoints_planner_B.err -= obj->TimeObj.StartTime;
9436 obj->MaxTimeInternal = obj->MaxTime - cartesian_waypoints_planner_B.err;
9437 if (obj->MaxNumIterationInternal <= 0.0) {
9438 cartesian_waypoints_planner_B.exitFlag = IterationLimitExceeded;
9439 }
9440
9441 if ((cartesian_waypoints_planner_B.exitFlag == IterationLimitExceeded) ||
9442 (cartesian_waypoints_planner_B.exitFlag == TimeLimitExceeded)) {
9443 cartesian_waypoints_planner_B.exitFlagPrev =
9444 cartesian_waypoints_planner_B.exitFlag;
9445 exitg1 = true;
9446 } else {
9447 args = obj->ExtraArgs;
9448 obj_0 = args->Robot;
9449 cartesian_waypoints_planner_B.ix = newseed->size[0];
9450 newseed->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
9451 cartes_emxEnsureCapacity_real_T(newseed, cartesian_waypoints_planner_B.ix);
9452 cartesian_waypoints_planner_B.nx = static_cast<int32_T>
9453 (obj_0->PositionNumber);
9454 for (cartesian_waypoints_planner_B.ix = 0;
9455 cartesian_waypoints_planner_B.ix < cartesian_waypoints_planner_B.nx;
9456 cartesian_waypoints_planner_B.ix++) {
9457 newseed->data[cartesian_waypoints_planner_B.ix] = 0.0;
9458 }
9459
9460 cartesian_waypoints_planner_B.err = obj_0->NumBodies;
9461 cartesian_waypoints_planner_B.c_a = static_cast<int32_T>
9462 (cartesian_waypoints_planner_B.err) - 1;
9463 for (cartesian_waypoints_planner_B.i_f = 0;
9464 cartesian_waypoints_planner_B.i_f <=
9465 cartesian_waypoints_planner_B.c_a; cartesian_waypoints_planner_B.i_f
9466 ++) {
9467 cartesian_waypoints_planner_B.err = obj_0->
9468 PositionDoFMap[cartesian_waypoints_planner_B.i_f];
9469 cartesian_waypoints_planner_B.iter = obj_0->
9470 PositionDoFMap[cartesian_waypoints_planner_B.i_f + 8];
9471 if (cartesian_waypoints_planner_B.err <=
9472 cartesian_waypoints_planner_B.iter) {
9473 obj_1 = obj_0->Bodies[cartesian_waypoints_planner_B.i_f]
9474 ->JointInternal;
9475 if (static_cast<int32_T>(obj_1->PositionNumber) == 0) {
9476 cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];
9477 qi->size[0] = 1;
9478 qi->size[1] = 1;
9479 cartes_emxEnsureCapacity_real_T(qi, cartesian_waypoints_planner_B.ix);
9480 qi->data[0] = (rtNaN);
9481 } else {
9482 cartesian_waypoints_planner_B.nx = obj_1->
9483 PositionLimitsInternal->size[0];
9484 cartesian_waypoints_planner_B.ix = ub->size[0];
9485 ub->size[0] = cartesian_waypoints_planner_B.nx;
9486 cartes_emxEnsureCapacity_real_T(ub, cartesian_waypoints_planner_B.ix);
9487 for (cartesian_waypoints_planner_B.ix = 0;
9488 cartesian_waypoints_planner_B.ix <
9489 cartesian_waypoints_planner_B.nx;
9490 cartesian_waypoints_planner_B.ix++) {
9491 ub->data[cartesian_waypoints_planner_B.ix] =
9492 obj_1->PositionLimitsInternal->
9493 data[cartesian_waypoints_planner_B.ix +
9494 obj_1->PositionLimitsInternal->size[0]];
9495 }
9496
9497 cartesian_waypoints_planner_B.nx = obj_1->
9498 PositionLimitsInternal->size[0];
9499 cartesian_waypoints_planner_B.ix = lb->size[0];
9500 lb->size[0] = cartesian_waypoints_planner_B.nx;
9501 cartes_emxEnsureCapacity_real_T(lb, cartesian_waypoints_planner_B.ix);
9502 for (cartesian_waypoints_planner_B.ix = 0;
9503 cartesian_waypoints_planner_B.ix <
9504 cartesian_waypoints_planner_B.nx;
9505 cartesian_waypoints_planner_B.ix++) {
9506 lb->data[cartesian_waypoints_planner_B.ix] =
9507 obj_1->PositionLimitsInternal->
9508 data[cartesian_waypoints_planner_B.ix];
9509 }
9510
9511 cartesian_waypoints_pl_isfinite(lb, x_tmp);
9512 cartesian_waypoints_planner_B.y_da = true;
9513 cartesian_waypoints_planner_B.ix = 0;
9514 exitg2 = false;
9515 while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=
9516 x_tmp->size[0])) {
9517 if (!x_tmp->data[cartesian_waypoints_planner_B.ix]) {
9518 cartesian_waypoints_planner_B.y_da = false;
9519 exitg2 = true;
9520 } else {
9521 cartesian_waypoints_planner_B.ix++;
9522 }
9523 }
9524
9525 guard1 = false;
9526 guard2 = false;
9527 guard3 = false;
9528 if (cartesian_waypoints_planner_B.y_da) {
9529 cartesian_waypoints_pl_isfinite(ub, x);
9530 cartesian_waypoints_planner_B.y_da = true;
9531 cartesian_waypoints_planner_B.ix = 0;
9532 exitg2 = false;
9533 while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=
9534 x->size[0])) {
9535 if (!x->data[cartesian_waypoints_planner_B.ix]) {
9536 cartesian_waypoints_planner_B.y_da = false;
9537 exitg2 = true;
9538 } else {
9539 cartesian_waypoints_planner_B.ix++;
9540 }
9541 }
9542
9543 if (cartesian_waypoints_planner_B.y_da) {
9544 cartesian_waypoints_plan_rand_e(obj_1->PositionNumber, rn);
9545 cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];
9546 qi->size[0] = lb->size[0];
9547 qi->size[1] = 1;
9548 cartes_emxEnsureCapacity_real_T(qi,
9549 cartesian_waypoints_planner_B.ix);
9550 cartesian_waypoints_planner_B.nx = lb->size[0] - 1;
9551 for (cartesian_waypoints_planner_B.ix = 0;
9552 cartesian_waypoints_planner_B.ix <=
9553 cartesian_waypoints_planner_B.nx;
9554 cartesian_waypoints_planner_B.ix++) {
9555 qi->data[cartesian_waypoints_planner_B.ix] = (ub->
9556 data[cartesian_waypoints_planner_B.ix] - lb->
9557 data[cartesian_waypoints_planner_B.ix]) * rn->
9558 data[cartesian_waypoints_planner_B.ix] + lb->
9559 data[cartesian_waypoints_planner_B.ix];
9560 }
9561 } else {
9562 guard3 = true;
9563 }
9564 } else {
9565 guard3 = true;
9566 }
9567
9568 if (guard3) {
9569 cartesian_waypoints_planner_B.y_da = true;
9570 cartesian_waypoints_planner_B.ix = 0;
9571 exitg2 = false;
9572 while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=
9573 x_tmp->size[0])) {
9574 if (!x_tmp->data[cartesian_waypoints_planner_B.ix]) {
9575 cartesian_waypoints_planner_B.y_da = false;
9576 exitg2 = true;
9577 } else {
9578 cartesian_waypoints_planner_B.ix++;
9579 }
9580 }
9581
9582 if (cartesian_waypoints_planner_B.y_da) {
9583 cartesian_waypoints_pl_isfinite(ub, x);
9584 cartesian_waypoints_planner_B.ix = x_0->size[0];
9585 x_0->size[0] = x->size[0];
9586 car_emxEnsureCapacity_boolean_T(x_0,
9587 cartesian_waypoints_planner_B.ix);
9588 cartesian_waypoints_planner_B.nx = x->size[0];
9589 for (cartesian_waypoints_planner_B.ix = 0;
9590 cartesian_waypoints_planner_B.ix <
9591 cartesian_waypoints_planner_B.nx;
9592 cartesian_waypoints_planner_B.ix++) {
9593 x_0->data[cartesian_waypoints_planner_B.ix] = !x->
9594 data[cartesian_waypoints_planner_B.ix];
9595 }
9596
9597 if (cartesian_waypoints_planner_any(x_0)) {
9598 cartesian_waypoints_planner_B.ub[0] = lb->size[0];
9599 cartesian_waypoints_planner_B.ub[1] = 1.0;
9600 cartesian_waypoints_plann_randn
9601 (cartesian_waypoints_planner_B.ub, qi);
9602 cartesian_waypoints_planner_B.nx = qi->size[0] - 1;
9603 cartesian_waypoints_planner_B.ix = e->size[0] * e->size[1];
9604 e->size[0] = qi->size[0];
9605 e->size[1] = 1;
9606 cartes_emxEnsureCapacity_real_T(e,
9607 cartesian_waypoints_planner_B.ix);
9608 for (cartesian_waypoints_planner_B.ix = 0;
9609 cartesian_waypoints_planner_B.ix <=
9610 cartesian_waypoints_planner_B.nx;
9611 cartesian_waypoints_planner_B.ix++) {
9612 e->data[cartesian_waypoints_planner_B.ix] = fabs(qi->
9613 data[cartesian_waypoints_planner_B.ix]);
9614 }
9615
9616 cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];
9617 qi->size[0] = lb->size[0];
9618 qi->size[1] = 1;
9619 cartes_emxEnsureCapacity_real_T(qi,
9620 cartesian_waypoints_planner_B.ix);
9621 cartesian_waypoints_planner_B.nx = lb->size[0] - 1;
9622 for (cartesian_waypoints_planner_B.ix = 0;
9623 cartesian_waypoints_planner_B.ix <=
9624 cartesian_waypoints_planner_B.nx;
9625 cartesian_waypoints_planner_B.ix++) {
9626 qi->data[cartesian_waypoints_planner_B.ix] = lb->
9627 data[cartesian_waypoints_planner_B.ix] + e->
9628 data[cartesian_waypoints_planner_B.ix];
9629 }
9630 } else {
9631 guard2 = true;
9632 }
9633 } else {
9634 guard2 = true;
9635 }
9636 }
9637
9638 if (guard2) {
9639 cartesian_waypoints_planner_B.ix = x_tmp_0->size[0];
9640 x_tmp_0->size[0] = x_tmp->size[0];
9641 car_emxEnsureCapacity_boolean_T(x_tmp_0,
9642 cartesian_waypoints_planner_B.ix);
9643 cartesian_waypoints_planner_B.nx = x_tmp->size[0];
9644 for (cartesian_waypoints_planner_B.ix = 0;
9645 cartesian_waypoints_planner_B.ix <
9646 cartesian_waypoints_planner_B.nx;
9647 cartesian_waypoints_planner_B.ix++) {
9648 x_tmp_0->data[cartesian_waypoints_planner_B.ix] = !x_tmp->
9649 data[cartesian_waypoints_planner_B.ix];
9650 }
9651
9652 if (cartesian_waypoints_planner_any(x_tmp_0)) {
9653 cartesian_waypoints_pl_isfinite(ub, x);
9654 cartesian_waypoints_planner_B.y_da = true;
9655 cartesian_waypoints_planner_B.ix = 0;
9656 exitg2 = false;
9657 while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=
9658 x->size[0])) {
9659 if (!x->data[cartesian_waypoints_planner_B.ix]) {
9660 cartesian_waypoints_planner_B.y_da = false;
9661 exitg2 = true;
9662 } else {
9663 cartesian_waypoints_planner_B.ix++;
9664 }
9665 }
9666
9667 if (cartesian_waypoints_planner_B.y_da) {
9668 cartesian_waypoints_planner_B.ub[0] = ub->size[0];
9669 cartesian_waypoints_planner_B.ub[1] = 1.0;
9670 cartesian_waypoints_plann_randn
9671 (cartesian_waypoints_planner_B.ub, qi);
9672 cartesian_waypoints_planner_B.nx = qi->size[0] - 1;
9673 cartesian_waypoints_planner_B.ix = e->size[0] * e->size[1];
9674 e->size[0] = qi->size[0];
9675 e->size[1] = 1;
9676 cartes_emxEnsureCapacity_real_T(e,
9677 cartesian_waypoints_planner_B.ix);
9678 for (cartesian_waypoints_planner_B.ix = 0;
9679 cartesian_waypoints_planner_B.ix <=
9680 cartesian_waypoints_planner_B.nx;
9681 cartesian_waypoints_planner_B.ix++) {
9682 e->data[cartesian_waypoints_planner_B.ix] = fabs(qi->
9683 data[cartesian_waypoints_planner_B.ix]);
9684 }
9685
9686 cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];
9687 qi->size[0] = ub->size[0];
9688 qi->size[1] = 1;
9689 cartes_emxEnsureCapacity_real_T(qi,
9690 cartesian_waypoints_planner_B.ix);
9691 cartesian_waypoints_planner_B.nx = ub->size[0] - 1;
9692 for (cartesian_waypoints_planner_B.ix = 0;
9693 cartesian_waypoints_planner_B.ix <=
9694 cartesian_waypoints_planner_B.nx;
9695 cartesian_waypoints_planner_B.ix++) {
9696 qi->data[cartesian_waypoints_planner_B.ix] = ub->
9697 data[cartesian_waypoints_planner_B.ix] - e->
9698 data[cartesian_waypoints_planner_B.ix];
9699 }
9700 } else {
9701 guard1 = true;
9702 }
9703 } else {
9704 guard1 = true;
9705 }
9706 }
9707
9708 if (guard1) {
9709 cartesian_waypoints_planner_B.ub[0] = ub->size[0];
9710 cartesian_waypoints_planner_B.ub[1] = 1.0;
9711 cartesian_waypoints_plann_randn(cartesian_waypoints_planner_B.ub,
9712 qi);
9713 }
9714 }
9715
9716 if (cartesian_waypoints_planner_B.err >
9717 cartesian_waypoints_planner_B.iter) {
9718 cartesian_waypoints_planner_B.nx = 0;
9719 cartesian_waypoints_planner_B.ix = 0;
9720 } else {
9721 cartesian_waypoints_planner_B.nx = static_cast<int32_T>
9722 (cartesian_waypoints_planner_B.err) - 1;
9723 cartesian_waypoints_planner_B.ix = static_cast<int32_T>
9724 (cartesian_waypoints_planner_B.iter);
9725 }
9726
9727 cartesian_waypoints_planner_B.unnamed_idx_1 =
9728 cartesian_waypoints_planner_B.ix - cartesian_waypoints_planner_B.nx;
9729 for (cartesian_waypoints_planner_B.ix = 0;
9730 cartesian_waypoints_planner_B.ix <
9731 cartesian_waypoints_planner_B.unnamed_idx_1;
9732 cartesian_waypoints_planner_B.ix++) {
9733 newseed->data[cartesian_waypoints_planner_B.nx +
9734 cartesian_waypoints_planner_B.ix] = qi->
9735 data[cartesian_waypoints_planner_B.ix];
9736 }
9737 }
9738 }
9739
9740 for (cartesian_waypoints_planner_B.ix = 0;
9741 cartesian_waypoints_planner_B.ix < 6;
9742 cartesian_waypoints_planner_B.ix++) {
9743 obj->SeedInternal[cartesian_waypoints_planner_B.ix] = newseed->
9744 data[cartesian_waypoints_planner_B.ix];
9745 }
9746
9747 DampedBFGSwGradientProjection_s(obj, cartesian_waypoints_planner_B.c_xSol,
9748 &cartesian_waypoints_planner_B.exitFlag,
9749 &cartesian_waypoints_planner_B.err, &cartesian_waypoints_planner_B.iter);
9750 if (cartesian_waypoints_planner_B.err < *solutionInfo_Error) {
9751 for (cartesian_waypoints_planner_B.i_f = 0;
9752 cartesian_waypoints_planner_B.i_f < 6;
9753 cartesian_waypoints_planner_B.i_f++) {
9754 xSol[cartesian_waypoints_planner_B.i_f] =
9755 cartesian_waypoints_planner_B.c_xSol[cartesian_waypoints_planner_B.i_f];
9756 }
9757
9758 *solutionInfo_Error = cartesian_waypoints_planner_B.err;
9759 cartesian_waypoints_planner_B.exitFlagPrev =
9760 cartesian_waypoints_planner_B.exitFlag;
9761 }
9762
9763 (*solutionInfo_RRAttempts)++;
9764 *solutionInfo_Iterations += cartesian_waypoints_planner_B.iter;
9765 }
9766 }
9767
9768 cartesian_way_emxFree_boolean_T(&x_0);
9769 cartesian_way_emxFree_boolean_T(&x_tmp_0);
9770 cartesian_way_emxFree_boolean_T(&x_tmp);
9771 cartesian_way_emxFree_boolean_T(&x);
9772 cartesian_waypoi_emxFree_real_T(&e);
9773 cartesian_waypoi_emxFree_real_T(&rn);
9774 cartesian_waypoi_emxFree_real_T(&lb);
9775 cartesian_waypoi_emxFree_real_T(&ub);
9776 cartesian_waypoi_emxFree_real_T(&qi);
9777 cartesian_waypoi_emxFree_real_T(&newseed);
9778 *solutionInfo_ExitFlag = cartesian_waypoints_planner_B.exitFlagPrev;
9779 if (*solutionInfo_Error < cartesian_waypoints_planner_B.tol) {
9780 solutionInfo_Status_size[0] = 1;
9781 solutionInfo_Status_size[1] = 7;
9782 for (cartesian_waypoints_planner_B.ix = 0; cartesian_waypoints_planner_B.ix <
9783 7; cartesian_waypoints_planner_B.ix++) {
9784 solutionInfo_Status_data[cartesian_waypoints_planner_B.ix] =
9785 tmp_0[cartesian_waypoints_planner_B.ix];
9786 }
9787 } else {
9788 solutionInfo_Status_size[0] = 1;
9789 solutionInfo_Status_size[1] = 14;
9790 for (cartesian_waypoints_planner_B.ix = 0; cartesian_waypoints_planner_B.ix <
9791 14; cartesian_waypoints_planner_B.ix++) {
9792 solutionInfo_Status_data[cartesian_waypoints_planner_B.ix] =
9793 tmp[cartesian_waypoints_planner_B.ix];
9794 }
9795 }
9796}
9797
9798static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
9799 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
9800 real_T QSol[6])
9801{
9802 emxArray_real_T_cartesian_way_T *bodyIndices;
9803 emxArray_real_T_cartesian_way_T *positionIndices;
9804 x_robotics_manip_internal_Rig_T *obj_0;
9805 emxArray_char_T_cartesian_way_T *endEffectorName;
9806 w_robotics_manip_internal_Rig_T *body;
9807 emxArray_real_T_cartesian_way_T *positionMap;
9808 emxArray_real_T_cartesian_way_T *e;
9809 emxArray_int32_T_cartesian_wa_T *h;
9810 emxArray_real_T_cartesian_way_T *y;
9811 emxArray_char_T_cartesian_way_T *bname;
9812 emxArray_real_T_cartesian_way_T *bodyIndices_0;
9813 boolean_T exitg1;
9814 c_inverseKinematics_setPoseGoal(obj, tform, weights);
9815 for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i < 6;
9816 cartesian_waypoints_planner_B.i++) {
9817 QSol[cartesian_waypoints_planner_B.i] =
9818 initialGuess[cartesian_waypoints_planner_B.i];
9819 }
9820
9821 cartesian_waypoi_emxInit_char_T(&endEffectorName, 2);
9822 RigidBodyTree_validateConfigu_e(obj->RigidBodyTreeInternal, QSol);
9823 cartes_NLPSolverInterface_solve(obj->Solver, QSol,
9824 cartesian_waypoints_planner_B.qvSolRaw, &cartesian_waypoints_planner_B.bid,
9825 &cartesian_waypoints_planner_B.numPositions,
9826 &cartesian_waypoints_planner_B.ndbl, &cartesian_waypoints_planner_B.apnd,
9827 cartesian_waypoints_planner_B.sol_Status_data,
9828 cartesian_waypoints_planner_B.sol_Status_size);
9829 obj_0 = obj->RigidBodyTreeInternal;
9830 cartesian_waypoints_planner_B.partialTrueCount = endEffectorName->size[0] *
9831 endEffectorName->size[1];
9832 endEffectorName->size[0] = 1;
9833 endEffectorName->size[1] = obj->Solver->ExtraArgs->BodyName->size[1];
9834 cartes_emxEnsureCapacity_char_T(endEffectorName,
9835 cartesian_waypoints_planner_B.partialTrueCount);
9836 cartesian_waypoints_planner_B.loop_ub = obj->Solver->ExtraArgs->BodyName->
9837 size[0] * obj->Solver->ExtraArgs->BodyName->size[1] - 1;
9838 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
9839 cartesian_waypoints_planner_B.partialTrueCount <=
9840 cartesian_waypoints_planner_B.loop_ub;
9841 cartesian_waypoints_planner_B.partialTrueCount++) {
9842 endEffectorName->data[cartesian_waypoints_planner_B.partialTrueCount] =
9843 obj->Solver->ExtraArgs->BodyName->
9844 data[cartesian_waypoints_planner_B.partialTrueCount];
9845 }
9846
9847 cartesian_waypoi_emxInit_real_T(&bodyIndices, 1);
9848 cartesian_waypoints_planner_B.partialTrueCount = bodyIndices->size[0];
9849 bodyIndices->size[0] = static_cast<int32_T>(obj_0->NumBodies);
9850 cartes_emxEnsureCapacity_real_T(bodyIndices,
9851 cartesian_waypoints_planner_B.partialTrueCount);
9852 cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>(obj_0->NumBodies);
9853 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
9854 cartesian_waypoints_planner_B.partialTrueCount <
9855 cartesian_waypoints_planner_B.loop_ub;
9856 cartesian_waypoints_planner_B.partialTrueCount++) {
9857 bodyIndices->data[cartesian_waypoints_planner_B.partialTrueCount] = 0.0;
9858 }
9859
9860 cartesian_waypoi_emxInit_char_T(&bname, 2);
9861 cartesian_waypoints_planner_B.bid = -1.0;
9862 cartesian_waypoints_planner_B.partialTrueCount = bname->size[0] * bname->size
9863 [1];
9864 bname->size[0] = 1;
9865 bname->size[1] = obj_0->Base.NameInternal->size[1];
9866 cartes_emxEnsureCapacity_char_T(bname,
9867 cartesian_waypoints_planner_B.partialTrueCount);
9868 cartesian_waypoints_planner_B.loop_ub = obj_0->Base.NameInternal->size[0] *
9869 obj_0->Base.NameInternal->size[1] - 1;
9870 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
9871 cartesian_waypoints_planner_B.partialTrueCount <=
9872 cartesian_waypoints_planner_B.loop_ub;
9873 cartesian_waypoints_planner_B.partialTrueCount++) {
9874 bname->data[cartesian_waypoints_planner_B.partialTrueCount] =
9875 obj_0->Base.NameInternal->
9876 data[cartesian_waypoints_planner_B.partialTrueCount];
9877 }
9878
9879 if (cartesian_waypoints_plan_strcmp(bname, endEffectorName)) {
9880 cartesian_waypoints_planner_B.bid = 0.0;
9881 } else {
9882 cartesian_waypoints_planner_B.numPositions = obj_0->NumBodies;
9883 cartesian_waypoints_planner_B.i = 0;
9884 exitg1 = false;
9885 while ((!exitg1) && (cartesian_waypoints_planner_B.i <= static_cast<int32_T>
9886 (cartesian_waypoints_planner_B.numPositions) - 1)) {
9887 body = obj_0->Bodies[cartesian_waypoints_planner_B.i];
9888 cartesian_waypoints_planner_B.partialTrueCount = bname->size[0] *
9889 bname->size[1];
9890 bname->size[0] = 1;
9891 bname->size[1] = body->NameInternal->size[1];
9892 cartes_emxEnsureCapacity_char_T(bname,
9893 cartesian_waypoints_planner_B.partialTrueCount);
9894 cartesian_waypoints_planner_B.loop_ub = body->NameInternal->size[0] *
9895 body->NameInternal->size[1] - 1;
9896 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
9897 cartesian_waypoints_planner_B.partialTrueCount <=
9898 cartesian_waypoints_planner_B.loop_ub;
9899 cartesian_waypoints_planner_B.partialTrueCount++) {
9900 bname->data[cartesian_waypoints_planner_B.partialTrueCount] =
9901 body->NameInternal->
9902 data[cartesian_waypoints_planner_B.partialTrueCount];
9903 }
9904
9905 if (cartesian_waypoints_plan_strcmp(bname, endEffectorName)) {
9906 cartesian_waypoints_planner_B.bid = static_cast<real_T>
9907 (cartesian_waypoints_planner_B.i) + 1.0;
9908 exitg1 = true;
9909 } else {
9910 cartesian_waypoints_planner_B.i++;
9911 }
9912 }
9913 }
9914
9915 cartesian_waypoi_emxFree_char_T(&bname);
9916 cartesian_waypoi_emxFree_char_T(&endEffectorName);
9917 if (cartesian_waypoints_planner_B.bid == 0.0) {
9918 cartesian_waypoints_planner_B.partialTrueCount = bodyIndices->size[0];
9919 bodyIndices->size[0] = 1;
9920 cartes_emxEnsureCapacity_real_T(bodyIndices,
9921 cartesian_waypoints_planner_B.partialTrueCount);
9922 bodyIndices->data[0] = 0.0;
9923 } else {
9924 body = obj_0->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.bid)
9925 - 1];
9926 cartesian_waypoints_planner_B.bid = 1.0;
9927 while (body->ParentIndex != 0.0) {
9928 bodyIndices->data[static_cast<int32_T>(cartesian_waypoints_planner_B.bid)
9929 - 1] = body->Index;
9930 body = obj_0->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
9931 cartesian_waypoints_planner_B.bid++;
9932 }
9933
9934 if (1.0 > cartesian_waypoints_planner_B.bid - 1.0) {
9935 cartesian_waypoints_planner_B.c_c = -1;
9936 } else {
9937 cartesian_waypoints_planner_B.c_c = static_cast<int32_T>
9938 (cartesian_waypoints_planner_B.bid - 1.0) - 1;
9939 }
9940
9941 cartesian_waypoi_emxInit_real_T(&bodyIndices_0, 1);
9942 cartesian_waypoints_planner_B.partialTrueCount = bodyIndices_0->size[0];
9943 bodyIndices_0->size[0] = cartesian_waypoints_planner_B.c_c + 3;
9944 cartes_emxEnsureCapacity_real_T(bodyIndices_0,
9945 cartesian_waypoints_planner_B.partialTrueCount);
9946 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
9947 cartesian_waypoints_planner_B.partialTrueCount <=
9948 cartesian_waypoints_planner_B.c_c;
9949 cartesian_waypoints_planner_B.partialTrueCount++) {
9950 bodyIndices_0->data[cartesian_waypoints_planner_B.partialTrueCount] =
9951 bodyIndices->data[cartesian_waypoints_planner_B.partialTrueCount];
9952 }
9953
9954 bodyIndices_0->data[cartesian_waypoints_planner_B.c_c + 1] = body->Index;
9955 bodyIndices_0->data[cartesian_waypoints_planner_B.c_c + 2] = 0.0;
9956 cartesian_waypoints_planner_B.partialTrueCount = bodyIndices->size[0];
9957 bodyIndices->size[0] = bodyIndices_0->size[0];
9958 cartes_emxEnsureCapacity_real_T(bodyIndices,
9959 cartesian_waypoints_planner_B.partialTrueCount);
9960 cartesian_waypoints_planner_B.loop_ub = bodyIndices_0->size[0];
9961 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
9962 cartesian_waypoints_planner_B.partialTrueCount <
9963 cartesian_waypoints_planner_B.loop_ub;
9964 cartesian_waypoints_planner_B.partialTrueCount++) {
9965 bodyIndices->data[cartesian_waypoints_planner_B.partialTrueCount] =
9966 bodyIndices_0->data[cartesian_waypoints_planner_B.partialTrueCount];
9967 }
9968
9969 cartesian_waypoi_emxFree_real_T(&bodyIndices_0);
9970 }
9971
9972 obj_0 = obj->RigidBodyTreeInternal;
9973 cartesian_waypoints_planner_B.c_c = bodyIndices->size[0] - 1;
9974 cartesian_waypoints_planner_B.loop_ub = 0;
9975 for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i <=
9976 cartesian_waypoints_planner_B.c_c; cartesian_waypoints_planner_B.i++) {
9977 if (bodyIndices->data[cartesian_waypoints_planner_B.i] != 0.0) {
9978 cartesian_waypoints_planner_B.loop_ub++;
9979 }
9980 }
9981
9982 cartesian_waypo_emxInit_int32_T(&h, 1);
9983 cartesian_waypoints_planner_B.partialTrueCount = h->size[0];
9984 h->size[0] = cartesian_waypoints_planner_B.loop_ub;
9985 carte_emxEnsureCapacity_int32_T(h,
9986 cartesian_waypoints_planner_B.partialTrueCount);
9987 cartesian_waypoints_planner_B.partialTrueCount = 0;
9988 for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i <=
9989 cartesian_waypoints_planner_B.c_c; cartesian_waypoints_planner_B.i++) {
9990 if (bodyIndices->data[cartesian_waypoints_planner_B.i] != 0.0) {
9991 h->data[cartesian_waypoints_planner_B.partialTrueCount] =
9992 cartesian_waypoints_planner_B.i + 1;
9993 cartesian_waypoints_planner_B.partialTrueCount++;
9994 }
9995 }
9996
9997 cartesian_waypoi_emxInit_real_T(&positionMap, 2);
9998 cartesian_waypoints_planner_B.partialTrueCount = positionMap->size[0] *
9999 positionMap->size[1];
10000 positionMap->size[0] = h->size[0];
10001 positionMap->size[1] = 2;
10002 cartes_emxEnsureCapacity_real_T(positionMap,
10003 cartesian_waypoints_planner_B.partialTrueCount);
10004 cartesian_waypoints_planner_B.loop_ub = h->size[0];
10005 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10006 cartesian_waypoints_planner_B.partialTrueCount <
10007 cartesian_waypoints_planner_B.loop_ub;
10008 cartesian_waypoints_planner_B.partialTrueCount++) {
10009 positionMap->data[cartesian_waypoints_planner_B.partialTrueCount] =
10010 obj_0->PositionDoFMap[static_cast<int32_T>(bodyIndices->data[h->
10011 data[cartesian_waypoints_planner_B.partialTrueCount] - 1]) - 1];
10012 }
10013
10014 cartesian_waypoints_planner_B.loop_ub = h->size[0];
10015 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10016 cartesian_waypoints_planner_B.partialTrueCount <
10017 cartesian_waypoints_planner_B.loop_ub;
10018 cartesian_waypoints_planner_B.partialTrueCount++) {
10019 positionMap->data[cartesian_waypoints_planner_B.partialTrueCount +
10020 positionMap->size[0]] = obj_0->PositionDoFMap[static_cast<int32_T>
10021 (bodyIndices->data[h->data[cartesian_waypoints_planner_B.partialTrueCount]
10022 - 1]) + 7];
10023 }
10024
10025 cartesian_waypo_emxFree_int32_T(&h);
10026 cartesian_waypoi_emxFree_real_T(&bodyIndices);
10027 cartesian_waypoi_emxInit_real_T(&positionIndices, 2);
10028 cartesian_waypoints_planner_B.partialTrueCount = positionIndices->size[0] *
10029 positionIndices->size[1];
10030 positionIndices->size[0] = 1;
10031 positionIndices->size[1] = static_cast<int32_T>(obj_0->PositionNumber);
10032 cartes_emxEnsureCapacity_real_T(positionIndices,
10033 cartesian_waypoints_planner_B.partialTrueCount);
10034 cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>
10035 (obj_0->PositionNumber) - 1;
10036 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10037 cartesian_waypoints_planner_B.partialTrueCount <=
10038 cartesian_waypoints_planner_B.loop_ub;
10039 cartesian_waypoints_planner_B.partialTrueCount++) {
10040 positionIndices->data[cartesian_waypoints_planner_B.partialTrueCount] = 0.0;
10041 }
10042
10043 cartesian_waypoints_planner_B.bid = 0.0;
10044 cartesian_waypoints_planner_B.c_c = positionMap->size[0] - 1;
10045 cartesian_waypoi_emxInit_real_T(&e, 2);
10046 cartesian_waypoi_emxInit_real_T(&y, 2);
10047 for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i <=
10048 cartesian_waypoints_planner_B.c_c; cartesian_waypoints_planner_B.i++) {
10049 cartesian_waypoints_planner_B.numPositions = (positionMap->
10050 data[cartesian_waypoints_planner_B.i + positionMap->size[0]] -
10051 positionMap->data[cartesian_waypoints_planner_B.i]) + 1.0;
10052 if (cartesian_waypoints_planner_B.numPositions > 0.0) {
10053 if (cartesian_waypoints_planner_B.numPositions < 1.0) {
10054 y->size[0] = 1;
10055 y->size[1] = 0;
10056 } else if (rtIsInf(cartesian_waypoints_planner_B.numPositions) && (1.0 ==
10057 cartesian_waypoints_planner_B.numPositions)) {
10058 cartesian_waypoints_planner_B.partialTrueCount = y->size[0] * y->size[1];
10059 y->size[0] = 1;
10060 y->size[1] = 1;
10061 cartes_emxEnsureCapacity_real_T(y,
10062 cartesian_waypoints_planner_B.partialTrueCount);
10063 y->data[0] = (rtNaN);
10064 } else {
10065 cartesian_waypoints_planner_B.partialTrueCount = y->size[0] * y->size[1];
10066 y->size[0] = 1;
10067 cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>(floor
10068 (cartesian_waypoints_planner_B.numPositions - 1.0));
10069 y->size[1] = cartesian_waypoints_planner_B.loop_ub + 1;
10070 cartes_emxEnsureCapacity_real_T(y,
10071 cartesian_waypoints_planner_B.partialTrueCount);
10072 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10073 cartesian_waypoints_planner_B.partialTrueCount <=
10074 cartesian_waypoints_planner_B.loop_ub;
10075 cartesian_waypoints_planner_B.partialTrueCount++) {
10076 y->data[cartesian_waypoints_planner_B.partialTrueCount] = static_cast<
10077 real_T>(cartesian_waypoints_planner_B.partialTrueCount) + 1.0;
10078 }
10079 }
10080
10081 if (rtIsNaN(positionMap->data[cartesian_waypoints_planner_B.i]) || rtIsNaN
10082 (positionMap->data[cartesian_waypoints_planner_B.i + positionMap->
10083 size[0]])) {
10084 cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];
10085 e->size[0] = 1;
10086 e->size[1] = 1;
10087 cartes_emxEnsureCapacity_real_T(e,
10088 cartesian_waypoints_planner_B.partialTrueCount);
10089 e->data[0] = (rtNaN);
10090 } else if (positionMap->data[cartesian_waypoints_planner_B.i +
10091 positionMap->size[0]] < positionMap->
10092 data[cartesian_waypoints_planner_B.i]) {
10093 e->size[0] = 1;
10094 e->size[1] = 0;
10095 } else if ((rtIsInf(positionMap->data[cartesian_waypoints_planner_B.i]) ||
10096 rtIsInf(positionMap->data[cartesian_waypoints_planner_B.i +
10097 positionMap->size[0]])) && (positionMap->
10098 data[cartesian_waypoints_planner_B.i + positionMap->size[0]] ==
10099 positionMap->data[cartesian_waypoints_planner_B.i])) {
10100 cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];
10101 e->size[0] = 1;
10102 e->size[1] = 1;
10103 cartes_emxEnsureCapacity_real_T(e,
10104 cartesian_waypoints_planner_B.partialTrueCount);
10105 e->data[0] = (rtNaN);
10106 } else if (floor(positionMap->data[cartesian_waypoints_planner_B.i]) ==
10107 positionMap->data[cartesian_waypoints_planner_B.i]) {
10108 cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];
10109 e->size[0] = 1;
10110 e->size[1] = static_cast<int32_T>(floor(positionMap->
10111 data[cartesian_waypoints_planner_B.i + positionMap->size[0]] -
10112 positionMap->data[cartesian_waypoints_planner_B.i])) + 1;
10113 cartes_emxEnsureCapacity_real_T(e,
10114 cartesian_waypoints_planner_B.partialTrueCount);
10115 cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>(floor
10116 (positionMap->data[cartesian_waypoints_planner_B.i + positionMap->
10117 size[0]] - positionMap->data[cartesian_waypoints_planner_B.i]));
10118 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10119 cartesian_waypoints_planner_B.partialTrueCount <=
10120 cartesian_waypoints_planner_B.loop_ub;
10121 cartesian_waypoints_planner_B.partialTrueCount++) {
10122 e->data[cartesian_waypoints_planner_B.partialTrueCount] =
10123 positionMap->data[cartesian_waypoints_planner_B.i] +
10124 static_cast<real_T>(cartesian_waypoints_planner_B.partialTrueCount);
10125 }
10126 } else {
10127 cartesian_waypoints_planner_B.ndbl = floor((positionMap->
10128 data[cartesian_waypoints_planner_B.i + positionMap->size[0]] -
10129 positionMap->data[cartesian_waypoints_planner_B.i]) + 0.5);
10130 cartesian_waypoints_planner_B.apnd = positionMap->
10131 data[cartesian_waypoints_planner_B.i] +
10132 cartesian_waypoints_planner_B.ndbl;
10133 cartesian_waypoints_planner_B.cdiff = cartesian_waypoints_planner_B.apnd
10134 - positionMap->data[cartesian_waypoints_planner_B.i +
10135 positionMap->size[0]];
10136 cartesian_waypoints_planner_B.u0 = fabs(positionMap->
10137 data[cartesian_waypoints_planner_B.i]);
10138 cartesian_waypoints_planner_B.u1 = fabs(positionMap->
10139 data[cartesian_waypoints_planner_B.i + positionMap->size[0]]);
10140 if ((cartesian_waypoints_planner_B.u0 > cartesian_waypoints_planner_B.u1)
10141 || rtIsNaN(cartesian_waypoints_planner_B.u1)) {
10142 cartesian_waypoints_planner_B.u1 = cartesian_waypoints_planner_B.u0;
10143 }
10144
10145 if (fabs(cartesian_waypoints_planner_B.cdiff) < 4.4408920985006262E-16 *
10146 cartesian_waypoints_planner_B.u1) {
10147 cartesian_waypoints_planner_B.ndbl++;
10148 cartesian_waypoints_planner_B.apnd = positionMap->
10149 data[cartesian_waypoints_planner_B.i + positionMap->size[0]];
10150 } else if (cartesian_waypoints_planner_B.cdiff > 0.0) {
10151 cartesian_waypoints_planner_B.apnd =
10152 (cartesian_waypoints_planner_B.ndbl - 1.0) + positionMap->
10153 data[cartesian_waypoints_planner_B.i];
10154 } else {
10155 cartesian_waypoints_planner_B.ndbl++;
10156 }
10157
10158 if (cartesian_waypoints_planner_B.ndbl >= 0.0) {
10159 cartesian_waypoints_planner_B.partialTrueCount = static_cast<int32_T>
10160 (cartesian_waypoints_planner_B.ndbl);
10161 } else {
10162 cartesian_waypoints_planner_B.partialTrueCount = 0;
10163 }
10164
10165 cartesian_waypoints_planner_B.loop_ub =
10166 cartesian_waypoints_planner_B.partialTrueCount - 1;
10167 cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];
10168 e->size[0] = 1;
10169 e->size[1] = cartesian_waypoints_planner_B.loop_ub + 1;
10170 cartes_emxEnsureCapacity_real_T(e,
10171 cartesian_waypoints_planner_B.partialTrueCount);
10172 if (cartesian_waypoints_planner_B.loop_ub + 1 > 0) {
10173 e->data[0] = positionMap->data[cartesian_waypoints_planner_B.i];
10174 if (cartesian_waypoints_planner_B.loop_ub + 1 > 1) {
10175 e->data[cartesian_waypoints_planner_B.loop_ub] =
10176 cartesian_waypoints_planner_B.apnd;
10177 cartesian_waypoints_planner_B.nm1d2 =
10178 ((cartesian_waypoints_planner_B.loop_ub < 0) +
10179 cartesian_waypoints_planner_B.loop_ub) >> 1;
10180 cartesian_waypoints_planner_B.c_k =
10181 cartesian_waypoints_planner_B.nm1d2 - 2;
10182 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10183 cartesian_waypoints_planner_B.partialTrueCount <=
10184 cartesian_waypoints_planner_B.c_k;
10185 cartesian_waypoints_planner_B.partialTrueCount++) {
10186 cartesian_waypoints_planner_B.k_n =
10187 cartesian_waypoints_planner_B.partialTrueCount + 1;
10188 e->data[cartesian_waypoints_planner_B.k_n] = positionMap->
10189 data[cartesian_waypoints_planner_B.i] + static_cast<real_T>
10190 (cartesian_waypoints_planner_B.k_n);
10191 e->data[cartesian_waypoints_planner_B.loop_ub -
10192 cartesian_waypoints_planner_B.k_n] =
10193 cartesian_waypoints_planner_B.apnd - static_cast<real_T>
10194 (cartesian_waypoints_planner_B.k_n);
10195 }
10196
10197 if (cartesian_waypoints_planner_B.nm1d2 << 1 ==
10198 cartesian_waypoints_planner_B.loop_ub) {
10199 e->data[cartesian_waypoints_planner_B.nm1d2] = (positionMap->
10200 data[cartesian_waypoints_planner_B.i] +
10201 cartesian_waypoints_planner_B.apnd) / 2.0;
10202 } else {
10203 e->data[cartesian_waypoints_planner_B.nm1d2] = positionMap->
10204 data[cartesian_waypoints_planner_B.i] + static_cast<real_T>
10205 (cartesian_waypoints_planner_B.nm1d2);
10206 e->data[cartesian_waypoints_planner_B.nm1d2 + 1] =
10207 cartesian_waypoints_planner_B.apnd - static_cast<real_T>
10208 (cartesian_waypoints_planner_B.nm1d2);
10209 }
10210 }
10211 }
10212 }
10213
10214 cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];
10215 cartesian_waypoints_planner_B.loop_ub =
10216 cartesian_waypoints_planner_B.partialTrueCount - 1;
10217 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10218 cartesian_waypoints_planner_B.partialTrueCount <=
10219 cartesian_waypoints_planner_B.loop_ub;
10220 cartesian_waypoints_planner_B.partialTrueCount++) {
10221 positionIndices->data[static_cast<int32_T>
10222 (cartesian_waypoints_planner_B.bid + y->
10223 data[cartesian_waypoints_planner_B.partialTrueCount]) - 1] = e->
10224 data[cartesian_waypoints_planner_B.partialTrueCount];
10225 }
10226
10227 cartesian_waypoints_planner_B.bid +=
10228 cartesian_waypoints_planner_B.numPositions;
10229 }
10230 }
10231
10232 cartesian_waypoi_emxFree_real_T(&y);
10233 cartesian_waypoi_emxFree_real_T(&e);
10234 cartesian_waypoi_emxFree_real_T(&positionMap);
10235 if (1.0 > cartesian_waypoints_planner_B.bid) {
10236 positionIndices->size[1] = 0;
10237 } else {
10238 cartesian_waypoints_planner_B.partialTrueCount = positionIndices->size[0] *
10239 positionIndices->size[1];
10240 positionIndices->size[1] = static_cast<int32_T>
10241 (cartesian_waypoints_planner_B.bid);
10242 cartes_emxEnsureCapacity_real_T(positionIndices,
10243 cartesian_waypoints_planner_B.partialTrueCount);
10244 }
10245
10246 cartesian_waypoints_planner_B.loop_ub = positionIndices->size[0] *
10247 positionIndices->size[1];
10248 for (cartesian_waypoints_planner_B.partialTrueCount = 0;
10249 cartesian_waypoints_planner_B.partialTrueCount <
10250 cartesian_waypoints_planner_B.loop_ub;
10251 cartesian_waypoints_planner_B.partialTrueCount++) {
10252 QSol[static_cast<int32_T>(positionIndices->
10253 data[cartesian_waypoints_planner_B.partialTrueCount]) - 1] =
10254 cartesian_waypoints_planner_B.qvSolRaw[static_cast<int32_T>
10255 (positionIndices->data[cartesian_waypoints_planner_B.partialTrueCount]) -
10256 1];
10257 }
10258
10259 cartesian_waypoi_emxFree_real_T(&positionIndices);
10260}
10261
10262static void cartesian_w_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
10263 **pEmxArray, int32_T numDimensions)
10264{
10265 emxArray_f_cell_wrap_cartesia_T *emxArray;
10266 int32_T i;
10267 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)malloc(sizeof
10268 (emxArray_f_cell_wrap_cartesia_T));
10269 emxArray = *pEmxArray;
10270 emxArray->data = (f_cell_wrap_cartesian_waypoin_T *)NULL;
10271 emxArray->numDimensions = numDimensions;
10272 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
10273 emxArray->allocatedSize = 0;
10274 emxArray->canFreeData = true;
10275 for (i = 0; i < numDimensions; i++) {
10276 emxArray->size[i] = 0;
10277 }
10278}
10279
10280static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
10281 *emxArray, int32_T oldNumel)
10282{
10283 int32_T newNumel;
10284 int32_T i;
10285 void *newData;
10286 if (oldNumel < 0) {
10287 oldNumel = 0;
10288 }
10289
10290 newNumel = 1;
10291 for (i = 0; i < emxArray->numDimensions; i++) {
10292 newNumel *= emxArray->size[i];
10293 }
10294
10295 if (newNumel > emxArray->allocatedSize) {
10296 i = emxArray->allocatedSize;
10297 if (i < 16) {
10298 i = 16;
10299 }
10300
10301 while (i < newNumel) {
10302 if (i > 1073741823) {
10303 i = MAX_int32_T;
10304 } else {
10305 i <<= 1;
10306 }
10307 }
10308
10309 newData = calloc(static_cast<uint32_T>(i), sizeof
10310 (f_cell_wrap_cartesian_waypoin_T));
10311 if (emxArray->data != NULL) {
10312 memcpy(newData, emxArray->data, sizeof(f_cell_wrap_cartesian_waypoin_T)
10313 * oldNumel);
10314 if (emxArray->canFreeData) {
10315 free(emxArray->data);
10316 }
10317 }
10318
10319 emxArray->data = (f_cell_wrap_cartesian_waypoin_T *)newData;
10320 emxArray->allocatedSize = i;
10321 emxArray->canFreeData = true;
10322 }
10323}
10324
10325static void ca_rigidBodyJoint_get_JointAxis(const
10326 c_rigidBodyJoint_cartesian_wa_T *obj, real_T ax[3])
10327{
10328 boolean_T b_bool;
10329 emxArray_char_T_cartesian_way_T *a;
10330 int32_T b_kstr;
10331 int32_T loop_ub;
10332 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
10333
10334 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
10335
10336 boolean_T guard1 = false;
10337 int32_T exitg1;
10338 cartesian_waypoi_emxInit_char_T(&a, 2);
10339 b_kstr = a->size[0] * a->size[1];
10340 a->size[0] = 1;
10341 a->size[1] = obj->Type->size[1];
10342 cartes_emxEnsureCapacity_char_T(a, b_kstr);
10343 loop_ub = obj->Type->size[0] * obj->Type->size[1] - 1;
10344 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
10345 a->data[b_kstr] = obj->Type->data[b_kstr];
10346 }
10347
10348 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
10349 cartesian_waypoints_planner_B.b_g[b_kstr] = tmp[b_kstr];
10350 }
10351
10352 b_bool = false;
10353 if (a->size[1] == 8) {
10354 b_kstr = 1;
10355 do {
10356 exitg1 = 0;
10357 if (b_kstr - 1 < 8) {
10358 loop_ub = b_kstr - 1;
10359 if (a->data[loop_ub] != cartesian_waypoints_planner_B.b_g[loop_ub]) {
10360 exitg1 = 1;
10361 } else {
10362 b_kstr++;
10363 }
10364 } else {
10365 b_bool = true;
10366 exitg1 = 1;
10367 }
10368 } while (exitg1 == 0);
10369 }
10370
10371 guard1 = false;
10372 if (b_bool) {
10373 guard1 = true;
10374 } else {
10375 b_kstr = a->size[0] * a->size[1];
10376 a->size[0] = 1;
10377 a->size[1] = obj->Type->size[1];
10378 cartes_emxEnsureCapacity_char_T(a, b_kstr);
10379 loop_ub = obj->Type->size[0] * obj->Type->size[1] - 1;
10380 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
10381 a->data[b_kstr] = obj->Type->data[b_kstr];
10382 }
10383
10384 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
10385 cartesian_waypoints_planner_B.b_nb[b_kstr] = tmp_0[b_kstr];
10386 }
10387
10388 b_bool = false;
10389 if (a->size[1] == 9) {
10390 b_kstr = 1;
10391 do {
10392 exitg1 = 0;
10393 if (b_kstr - 1 < 9) {
10394 loop_ub = b_kstr - 1;
10395 if (a->data[loop_ub] != cartesian_waypoints_planner_B.b_nb[loop_ub]) {
10396 exitg1 = 1;
10397 } else {
10398 b_kstr++;
10399 }
10400 } else {
10401 b_bool = true;
10402 exitg1 = 1;
10403 }
10404 } while (exitg1 == 0);
10405 }
10406
10407 if (b_bool) {
10408 guard1 = true;
10409 } else {
10410 ax[0] = (rtNaN);
10411 ax[1] = (rtNaN);
10412 ax[2] = (rtNaN);
10413 }
10414 }
10415
10416 if (guard1) {
10417 ax[0] = obj->JointAxisInternal[0];
10418 ax[1] = obj->JointAxisInternal[1];
10419 ax[2] = obj->JointAxisInternal[2];
10420 }
10421
10422 cartesian_waypoi_emxFree_char_T(&a);
10423}
10424
10425static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,
10426 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree)
10427{
10428 n_robotics_manip_internal_Rig_T *body;
10429 emxArray_char_T_cartesian_way_T *switch_expression;
10430 boolean_T b_bool;
10431 int32_T loop_ub;
10432 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
10433 };
10434
10435 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
10436
10437 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
10438
10439 int32_T exitg1;
10440 cartesian_waypoints_planner_B.n = obj->NumBodies;
10441 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10442 cartesian_waypoints_planner_B.b_kstr_og < 16;
10443 cartesian_waypoints_planner_B.b_kstr_og++) {
10444 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.b_kstr_og] =
10445 tmp[cartesian_waypoints_planner_B.b_kstr_og];
10446 }
10447
10448 cartesian_waypoints_planner_B.b_kstr_og = Ttree->size[0] * Ttree->size[1];
10449 Ttree->size[0] = 1;
10450 cartesian_waypoints_planner_B.e_p = static_cast<int32_T>
10451 (cartesian_waypoints_planner_B.n);
10452 Ttree->size[1] = cartesian_waypoints_planner_B.e_p;
10453 c_emxEnsureCapacity_f_cell_wrap(Ttree, cartesian_waypoints_planner_B.b_kstr_og);
10454 if (cartesian_waypoints_planner_B.e_p != 0) {
10455 cartesian_waypoints_planner_B.ntilecols = cartesian_waypoints_planner_B.e_p
10456 - 1;
10457 if (0 <= cartesian_waypoints_planner_B.ntilecols) {
10458 memcpy(&cartesian_waypoints_planner_B.expl_temp.f1[0],
10459 &cartesian_waypoints_planner_B.c_f1[0], sizeof(real_T) << 4U);
10460 }
10461
10462 for (cartesian_waypoints_planner_B.b_jtilecol = 0;
10463 cartesian_waypoints_planner_B.b_jtilecol <=
10464 cartesian_waypoints_planner_B.ntilecols;
10465 cartesian_waypoints_planner_B.b_jtilecol++) {
10466 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol] =
10467 cartesian_waypoints_planner_B.expl_temp;
10468 }
10469 }
10470
10471 cartesian_waypoints_planner_B.k = 1.0;
10472 cartesian_waypoints_planner_B.ntilecols = static_cast<int32_T>
10473 (cartesian_waypoints_planner_B.n) - 1;
10474 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
10475 if (0 <= cartesian_waypoints_planner_B.ntilecols) {
10476 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10477 cartesian_waypoints_planner_B.b_kstr_og < 5;
10478 cartesian_waypoints_planner_B.b_kstr_og++) {
10479 cartesian_waypoints_planner_B.b_gf[cartesian_waypoints_planner_B.b_kstr_og]
10480 = tmp_0[cartesian_waypoints_planner_B.b_kstr_og];
10481 }
10482 }
10483
10484 for (cartesian_waypoints_planner_B.b_jtilecol = 0;
10485 cartesian_waypoints_planner_B.b_jtilecol <=
10486 cartesian_waypoints_planner_B.ntilecols;
10487 cartesian_waypoints_planner_B.b_jtilecol++) {
10488 body = obj->Bodies[cartesian_waypoints_planner_B.b_jtilecol];
10489 cartesian_waypoints_planner_B.n = body->JointInternal.PositionNumber;
10490 cartesian_waypoints_planner_B.n += cartesian_waypoints_planner_B.k;
10491 if (cartesian_waypoints_planner_B.k > cartesian_waypoints_planner_B.n - 1.0)
10492 {
10493 cartesian_waypoints_planner_B.e_p = 0;
10494 cartesian_waypoints_planner_B.d_i = 0;
10495 } else {
10496 cartesian_waypoints_planner_B.e_p = static_cast<int32_T>
10497 (cartesian_waypoints_planner_B.k) - 1;
10498 cartesian_waypoints_planner_B.d_i = static_cast<int32_T>
10499 (cartesian_waypoints_planner_B.n - 1.0);
10500 }
10501
10502 cartesian_waypoints_planner_B.b_kstr_og = switch_expression->size[0] *
10503 switch_expression->size[1];
10504 switch_expression->size[0] = 1;
10505 switch_expression->size[1] = body->JointInternal.Type->size[1];
10506 cartes_emxEnsureCapacity_char_T(switch_expression,
10507 cartesian_waypoints_planner_B.b_kstr_og);
10508 loop_ub = body->JointInternal.Type->size[0] * body->JointInternal.Type->
10509 size[1] - 1;
10510 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10511 cartesian_waypoints_planner_B.b_kstr_og <= loop_ub;
10512 cartesian_waypoints_planner_B.b_kstr_og++) {
10513 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_og] =
10514 body->JointInternal.Type->data[cartesian_waypoints_planner_B.b_kstr_og];
10515 }
10516
10517 b_bool = false;
10518 if (switch_expression->size[1] == 5) {
10519 cartesian_waypoints_planner_B.b_kstr_og = 1;
10520 do {
10521 exitg1 = 0;
10522 if (cartesian_waypoints_planner_B.b_kstr_og - 1 < 5) {
10523 loop_ub = cartesian_waypoints_planner_B.b_kstr_og - 1;
10524 if (switch_expression->data[loop_ub] !=
10525 cartesian_waypoints_planner_B.b_gf[loop_ub]) {
10526 exitg1 = 1;
10527 } else {
10528 cartesian_waypoints_planner_B.b_kstr_og++;
10529 }
10530 } else {
10531 b_bool = true;
10532 exitg1 = 1;
10533 }
10534 } while (exitg1 == 0);
10535 }
10536
10537 if (b_bool) {
10538 cartesian_waypoints_planner_B.b_kstr_og = 0;
10539 } else {
10540 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10541 cartesian_waypoints_planner_B.b_kstr_og < 8;
10542 cartesian_waypoints_planner_B.b_kstr_og++) {
10543 cartesian_waypoints_planner_B.b_a[cartesian_waypoints_planner_B.b_kstr_og]
10544 = tmp_1[cartesian_waypoints_planner_B.b_kstr_og];
10545 }
10546
10547 b_bool = false;
10548 if (switch_expression->size[1] == 8) {
10549 cartesian_waypoints_planner_B.b_kstr_og = 1;
10550 do {
10551 exitg1 = 0;
10552 if (cartesian_waypoints_planner_B.b_kstr_og - 1 < 8) {
10553 loop_ub = cartesian_waypoints_planner_B.b_kstr_og - 1;
10554 if (switch_expression->data[loop_ub] !=
10555 cartesian_waypoints_planner_B.b_a[loop_ub]) {
10556 exitg1 = 1;
10557 } else {
10558 cartesian_waypoints_planner_B.b_kstr_og++;
10559 }
10560 } else {
10561 b_bool = true;
10562 exitg1 = 1;
10563 }
10564 } while (exitg1 == 0);
10565 }
10566
10567 if (b_bool) {
10568 cartesian_waypoints_planner_B.b_kstr_og = 1;
10569 } else {
10570 cartesian_waypoints_planner_B.b_kstr_og = -1;
10571 }
10572 }
10573
10574 switch (cartesian_waypoints_planner_B.b_kstr_og) {
10575 case 0:
10576 memset(&cartesian_waypoints_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);
10577 cartesian_waypoints_planner_B.c_f1[0] = 1.0;
10578 cartesian_waypoints_planner_B.c_f1[5] = 1.0;
10579 cartesian_waypoints_planner_B.c_f1[10] = 1.0;
10580 cartesian_waypoints_planner_B.c_f1[15] = 1.0;
10581 break;
10582
10583 case 1:
10584 ca_rigidBodyJoint_get_JointAxis(&body->JointInternal,
10585 cartesian_waypoints_planner_B.v_m4);
10586 cartesian_waypoints_planner_B.d_i -= cartesian_waypoints_planner_B.e_p;
10587 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10588 cartesian_waypoints_planner_B.b_kstr_og <
10589 cartesian_waypoints_planner_B.d_i;
10590 cartesian_waypoints_planner_B.b_kstr_og++) {
10591 cartesian_waypoints_planner_B.e_data[cartesian_waypoints_planner_B.b_kstr_og]
10592 = cartesian_waypoints_planner_B.e_p +
10593 cartesian_waypoints_planner_B.b_kstr_og;
10594 }
10595
10596 cartesian_waypoints_planner_B.result_data_i[0] =
10597 cartesian_waypoints_planner_B.v_m4[0];
10598 cartesian_waypoints_planner_B.result_data_i[1] =
10599 cartesian_waypoints_planner_B.v_m4[1];
10600 cartesian_waypoints_planner_B.result_data_i[2] =
10601 cartesian_waypoints_planner_B.v_m4[2];
10602 if (0 <= (cartesian_waypoints_planner_B.d_i != 0) - 1) {
10603 cartesian_waypoints_planner_B.result_data_i[3] =
10604 qvec[cartesian_waypoints_planner_B.e_data[0]];
10605 }
10606
10607 cartesian_waypoints_planner_B.k = 1.0 / sqrt
10608 ((cartesian_waypoints_planner_B.result_data_i[0] *
10609 cartesian_waypoints_planner_B.result_data_i[0] +
10610 cartesian_waypoints_planner_B.result_data_i[1] *
10611 cartesian_waypoints_planner_B.result_data_i[1]) +
10612 cartesian_waypoints_planner_B.result_data_i[2] *
10613 cartesian_waypoints_planner_B.result_data_i[2]);
10614 cartesian_waypoints_planner_B.v_m4[0] =
10615 cartesian_waypoints_planner_B.result_data_i[0] *
10616 cartesian_waypoints_planner_B.k;
10617 cartesian_waypoints_planner_B.v_m4[1] =
10618 cartesian_waypoints_planner_B.result_data_i[1] *
10619 cartesian_waypoints_planner_B.k;
10620 cartesian_waypoints_planner_B.v_m4[2] =
10621 cartesian_waypoints_planner_B.result_data_i[2] *
10622 cartesian_waypoints_planner_B.k;
10623 cartesian_waypoints_planner_B.k = cos
10624 (cartesian_waypoints_planner_B.result_data_i[3]);
10625 cartesian_waypoints_planner_B.sth_p = sin
10626 (cartesian_waypoints_planner_B.result_data_i[3]);
10627 cartesian_waypoints_planner_B.tempR_l[0] =
10628 cartesian_waypoints_planner_B.v_m4[0] *
10629 cartesian_waypoints_planner_B.v_m4[0] * (1.0 -
10630 cartesian_waypoints_planner_B.k) + cartesian_waypoints_planner_B.k;
10631 cartesian_waypoints_planner_B.tempR_tmp_o =
10632 cartesian_waypoints_planner_B.v_m4[1] *
10633 cartesian_waypoints_planner_B.v_m4[0] * (1.0 -
10634 cartesian_waypoints_planner_B.k);
10635 cartesian_waypoints_planner_B.tempR_tmp_l =
10636 cartesian_waypoints_planner_B.v_m4[2] *
10637 cartesian_waypoints_planner_B.sth_p;
10638 cartesian_waypoints_planner_B.tempR_l[1] =
10639 cartesian_waypoints_planner_B.tempR_tmp_o -
10640 cartesian_waypoints_planner_B.tempR_tmp_l;
10641 cartesian_waypoints_planner_B.tempR_tmp_k =
10642 cartesian_waypoints_planner_B.v_m4[2] *
10643 cartesian_waypoints_planner_B.v_m4[0] * (1.0 -
10644 cartesian_waypoints_planner_B.k);
10645 cartesian_waypoints_planner_B.tempR_tmp_j =
10646 cartesian_waypoints_planner_B.v_m4[1] *
10647 cartesian_waypoints_planner_B.sth_p;
10648 cartesian_waypoints_planner_B.tempR_l[2] =
10649 cartesian_waypoints_planner_B.tempR_tmp_k +
10650 cartesian_waypoints_planner_B.tempR_tmp_j;
10651 cartesian_waypoints_planner_B.tempR_l[3] =
10652 cartesian_waypoints_planner_B.tempR_tmp_o +
10653 cartesian_waypoints_planner_B.tempR_tmp_l;
10654 cartesian_waypoints_planner_B.tempR_l[4] =
10655 cartesian_waypoints_planner_B.v_m4[1] *
10656 cartesian_waypoints_planner_B.v_m4[1] * (1.0 -
10657 cartesian_waypoints_planner_B.k) + cartesian_waypoints_planner_B.k;
10658 cartesian_waypoints_planner_B.tempR_tmp_o =
10659 cartesian_waypoints_planner_B.v_m4[2] *
10660 cartesian_waypoints_planner_B.v_m4[1] * (1.0 -
10661 cartesian_waypoints_planner_B.k);
10662 cartesian_waypoints_planner_B.tempR_tmp_l =
10663 cartesian_waypoints_planner_B.v_m4[0] *
10664 cartesian_waypoints_planner_B.sth_p;
10665 cartesian_waypoints_planner_B.tempR_l[5] =
10666 cartesian_waypoints_planner_B.tempR_tmp_o -
10667 cartesian_waypoints_planner_B.tempR_tmp_l;
10668 cartesian_waypoints_planner_B.tempR_l[6] =
10669 cartesian_waypoints_planner_B.tempR_tmp_k -
10670 cartesian_waypoints_planner_B.tempR_tmp_j;
10671 cartesian_waypoints_planner_B.tempR_l[7] =
10672 cartesian_waypoints_planner_B.tempR_tmp_o +
10673 cartesian_waypoints_planner_B.tempR_tmp_l;
10674 cartesian_waypoints_planner_B.tempR_l[8] =
10675 cartesian_waypoints_planner_B.v_m4[2] *
10676 cartesian_waypoints_planner_B.v_m4[2] * (1.0 -
10677 cartesian_waypoints_planner_B.k) + cartesian_waypoints_planner_B.k;
10678 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10679 cartesian_waypoints_planner_B.b_kstr_og < 3;
10680 cartesian_waypoints_planner_B.b_kstr_og++) {
10681 cartesian_waypoints_planner_B.e_p =
10682 cartesian_waypoints_planner_B.b_kstr_og + 1;
10683 cartesian_waypoints_planner_B.R_h[cartesian_waypoints_planner_B.e_p - 1]
10684 = cartesian_waypoints_planner_B.tempR_l
10685 [(cartesian_waypoints_planner_B.e_p - 1) * 3];
10686 cartesian_waypoints_planner_B.e_p =
10687 cartesian_waypoints_planner_B.b_kstr_og + 1;
10688 cartesian_waypoints_planner_B.R_h[cartesian_waypoints_planner_B.e_p + 2]
10689 = cartesian_waypoints_planner_B.tempR_l
10690 [(cartesian_waypoints_planner_B.e_p - 1) * 3 + 1];
10691 cartesian_waypoints_planner_B.e_p =
10692 cartesian_waypoints_planner_B.b_kstr_og + 1;
10693 cartesian_waypoints_planner_B.R_h[cartesian_waypoints_planner_B.e_p + 5]
10694 = cartesian_waypoints_planner_B.tempR_l
10695 [(cartesian_waypoints_planner_B.e_p - 1) * 3 + 2];
10696 }
10697
10698 memset(&cartesian_waypoints_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);
10699 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10700 cartesian_waypoints_planner_B.b_kstr_og < 3;
10701 cartesian_waypoints_planner_B.b_kstr_og++) {
10702 cartesian_waypoints_planner_B.d_i =
10703 cartesian_waypoints_planner_B.b_kstr_og << 2;
10704 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_i] =
10705 cartesian_waypoints_planner_B.R_h[3 *
10706 cartesian_waypoints_planner_B.b_kstr_og];
10707 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_i + 1]
10708 = cartesian_waypoints_planner_B.R_h[3 *
10709 cartesian_waypoints_planner_B.b_kstr_og + 1];
10710 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_i + 2]
10711 = cartesian_waypoints_planner_B.R_h[3 *
10712 cartesian_waypoints_planner_B.b_kstr_og + 2];
10713 }
10714
10715 cartesian_waypoints_planner_B.c_f1[15] = 1.0;
10716 break;
10717
10718 default:
10719 ca_rigidBodyJoint_get_JointAxis(&body->JointInternal,
10720 cartesian_waypoints_planner_B.v_m4);
10721 memset(&cartesian_waypoints_planner_B.tempR_l[0], 0, 9U * sizeof(real_T));
10722 cartesian_waypoints_planner_B.tempR_l[0] = 1.0;
10723 cartesian_waypoints_planner_B.tempR_l[4] = 1.0;
10724 cartesian_waypoints_planner_B.tempR_l[8] = 1.0;
10725 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10726 cartesian_waypoints_planner_B.b_kstr_og < 3;
10727 cartesian_waypoints_planner_B.b_kstr_og++) {
10728 cartesian_waypoints_planner_B.d_i =
10729 cartesian_waypoints_planner_B.b_kstr_og << 2;
10730 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_i] =
10731 cartesian_waypoints_planner_B.tempR_l[3 *
10732 cartesian_waypoints_planner_B.b_kstr_og];
10733 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_i + 1]
10734 = cartesian_waypoints_planner_B.tempR_l[3 *
10735 cartesian_waypoints_planner_B.b_kstr_og + 1];
10736 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_i + 2]
10737 = cartesian_waypoints_planner_B.tempR_l[3 *
10738 cartesian_waypoints_planner_B.b_kstr_og + 2];
10739 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.b_kstr_og
10740 + 12] =
10741 cartesian_waypoints_planner_B.v_m4[cartesian_waypoints_planner_B.b_kstr_og]
10742 * qvec[cartesian_waypoints_planner_B.e_p];
10743 }
10744
10745 cartesian_waypoints_planner_B.c_f1[3] = 0.0;
10746 cartesian_waypoints_planner_B.c_f1[7] = 0.0;
10747 cartesian_waypoints_planner_B.c_f1[11] = 0.0;
10748 cartesian_waypoints_planner_B.c_f1[15] = 1.0;
10749 break;
10750 }
10751
10752 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10753 cartesian_waypoints_planner_B.b_kstr_og < 16;
10754 cartesian_waypoints_planner_B.b_kstr_og++) {
10755 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_og]
10756 = body->
10757 JointInternal.JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_og];
10758 }
10759
10760 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10761 cartesian_waypoints_planner_B.b_kstr_og < 16;
10762 cartesian_waypoints_planner_B.b_kstr_og++) {
10763 cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.b_kstr_og]
10764 = body->
10765 JointInternal.ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_og];
10766 }
10767
10768 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10769 cartesian_waypoints_planner_B.b_kstr_og < 4;
10770 cartesian_waypoints_planner_B.b_kstr_og++) {
10771 for (cartesian_waypoints_planner_B.e_p = 0;
10772 cartesian_waypoints_planner_B.e_p < 4;
10773 cartesian_waypoints_planner_B.e_p++) {
10774 cartesian_waypoints_planner_B.d_i = cartesian_waypoints_planner_B.e_p <<
10775 2;
10776 loop_ub = cartesian_waypoints_planner_B.b_kstr_og +
10777 cartesian_waypoints_planner_B.d_i;
10778 cartesian_waypoints_planner_B.a_m[loop_ub] = 0.0;
10779 cartesian_waypoints_planner_B.a_m[loop_ub] +=
10780 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_i] *
10781 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_og];
10782 cartesian_waypoints_planner_B.a_m[loop_ub] +=
10783 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_i +
10784 1] *
10785 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_og
10786 + 4];
10787 cartesian_waypoints_planner_B.a_m[loop_ub] +=
10788 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_i +
10789 2] *
10790 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_og
10791 + 8];
10792 cartesian_waypoints_planner_B.a_m[loop_ub] +=
10793 cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_i +
10794 3] *
10795 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_og
10796 + 12];
10797 }
10798
10799 for (cartesian_waypoints_planner_B.e_p = 0;
10800 cartesian_waypoints_planner_B.e_p < 4;
10801 cartesian_waypoints_planner_B.e_p++) {
10802 cartesian_waypoints_planner_B.d_i = cartesian_waypoints_planner_B.e_p <<
10803 2;
10804 loop_ub = cartesian_waypoints_planner_B.b_kstr_og +
10805 cartesian_waypoints_planner_B.d_i;
10806 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol].f1[loop_ub] = 0.0;
10807 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol].f1[loop_ub] +=
10808 cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_i] *
10809 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_og];
10810 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol].f1[loop_ub] +=
10811 cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_i +
10812 1] *
10813 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_og
10814 + 4];
10815 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol].f1[loop_ub] +=
10816 cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_i +
10817 2] *
10818 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_og
10819 + 8];
10820 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol].f1[loop_ub] +=
10821 cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_i +
10822 3] *
10823 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_og
10824 + 12];
10825 }
10826 }
10827
10828 cartesian_waypoints_planner_B.k = cartesian_waypoints_planner_B.n;
10829 if (body->ParentIndex > 0.0) {
10830 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10831 cartesian_waypoints_planner_B.b_kstr_og < 16;
10832 cartesian_waypoints_planner_B.b_kstr_og++) {
10833 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_og]
10834 = Ttree->data[static_cast<int32_T>(body->ParentIndex) - 1]
10835 .f1[cartesian_waypoints_planner_B.b_kstr_og];
10836 }
10837
10838 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10839 cartesian_waypoints_planner_B.b_kstr_og < 4;
10840 cartesian_waypoints_planner_B.b_kstr_og++) {
10841 for (cartesian_waypoints_planner_B.e_p = 0;
10842 cartesian_waypoints_planner_B.e_p < 4;
10843 cartesian_waypoints_planner_B.e_p++) {
10844 cartesian_waypoints_planner_B.d_i = cartesian_waypoints_planner_B.e_p <<
10845 2;
10846 loop_ub = cartesian_waypoints_planner_B.b_kstr_og +
10847 cartesian_waypoints_planner_B.d_i;
10848 cartesian_waypoints_planner_B.a_m[loop_ub] = 0.0;
10849 cartesian_waypoints_planner_B.a_m[loop_ub] += Ttree->
10850 data[cartesian_waypoints_planner_B.b_jtilecol]
10851 .f1[cartesian_waypoints_planner_B.d_i] *
10852 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_og];
10853 cartesian_waypoints_planner_B.a_m[loop_ub] += Ttree->
10854 data[cartesian_waypoints_planner_B.b_jtilecol]
10855 .f1[cartesian_waypoints_planner_B.d_i + 1] *
10856 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_og
10857 + 4];
10858 cartesian_waypoints_planner_B.a_m[loop_ub] += Ttree->
10859 data[cartesian_waypoints_planner_B.b_jtilecol]
10860 .f1[cartesian_waypoints_planner_B.d_i + 2] *
10861 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_og
10862 + 8];
10863 cartesian_waypoints_planner_B.a_m[loop_ub] += Ttree->
10864 data[cartesian_waypoints_planner_B.b_jtilecol]
10865 .f1[cartesian_waypoints_planner_B.d_i + 3] *
10866 cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_og
10867 + 12];
10868 }
10869 }
10870
10871 for (cartesian_waypoints_planner_B.b_kstr_og = 0;
10872 cartesian_waypoints_planner_B.b_kstr_og < 16;
10873 cartesian_waypoints_planner_B.b_kstr_og++) {
10874 Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]
10875 .f1[cartesian_waypoints_planner_B.b_kstr_og] =
10876 cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_og];
10877 }
10878 }
10879 }
10880
10881 cartesian_waypoi_emxFree_char_T(&switch_expression);
10882}
10883
10884static void cartesian_w_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
10885 **pEmxArray)
10886{
10887 if (*pEmxArray != (emxArray_f_cell_wrap_cartesia_T *)NULL) {
10888 if (((*pEmxArray)->data != (f_cell_wrap_cartesian_waypoin_T *)NULL) &&
10889 (*pEmxArray)->canFreeData) {
10890 free((*pEmxArray)->data);
10891 }
10892
10893 free((*pEmxArray)->size);
10894 free(*pEmxArray);
10895 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)NULL;
10896 }
10897}
10898
10899static void matlabCodegenHandle_matlabCodeg(ros_slros_internal_block_GetP_T *obj)
10900{
10901 if (!obj->matlabCodegenIsDeleted) {
10902 obj->matlabCodegenIsDeleted = true;
10903 }
10904}
10905
10906static void matlabCodegenHandle_matlab_evqu(robotics_slmanip_internal_b_e_T *obj)
10907{
10908 if (!obj->matlabCodegenIsDeleted) {
10909 obj->matlabCodegenIsDeleted = true;
10910 }
10911}
10912
10913static void cartesian_wa_SystemCore_release(b_inverseKinematics_cartesian_T *obj)
10914{
10915 if (obj->isInitialized == 1) {
10916 obj->isInitialized = 2;
10917 }
10918}
10919
10920static void cartesian_SystemCore_delete_evq(b_inverseKinematics_cartesian_T *obj)
10921{
10922 cartesian_wa_SystemCore_release(obj);
10923}
10924
10925static void matlabCodegenHandle_matlabC_evq(b_inverseKinematics_cartesian_T *obj)
10926{
10927 if (!obj->matlabCodegenIsDeleted) {
10928 obj->matlabCodegenIsDeleted = true;
10929 cartesian_SystemCore_delete_evq(obj);
10930 }
10931}
10932
10933static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian__e_T
10934 *pStruct)
10935{
10936 cartesian_waypoi_emxFree_char_T(&pStruct->Type);
10937 cartesian_waypoi_emxFree_real_T(&pStruct->MotionSubspace);
10938 cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);
10939 cartesian_waypoi_emxFree_real_T(&pStruct->PositionLimitsInternal);
10940 cartesian_waypoi_emxFree_real_T(&pStruct->HomePositionInternal);
10941}
10942
10943static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
10944 *pStruct)
10945{
10946 cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);
10947 emxFreeStruct_c_rigidBodyJoint(&pStruct->JointInternal);
10948}
10949
10950static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
10951 *pStruct)
10952{
10953 emxFreeStruct_v_robotics_manip_(&pStruct->Base);
10954}
10955
10956static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
10957 *pStruct)
10958{
10959 cartesian_waypoi_emxFree_real_T(&pStruct->Limits);
10960}
10961
10962static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_b_e_T
10963 *pStruct)
10964{
10965 emxFreeStruct_y_robotics_manip_(&pStruct->TreeInternal);
10966 emxFreeStruct_b_inverseKinemati(&pStruct->IKInternal);
10967}
10968
10969static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
10970 *pStruct)
10971{
10972 cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);
10973}
10974
10975static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
10976 *pStruct)
10977{
10978 emxFreeStruct_w_robotics_manip_(&pStruct->Base);
10979}
10980
10981static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
10982 *pStruct)
10983{
10984 cartesian_waypoi_emxFree_char_T(&pStruct->BodyName);
10985 cartesian_waypoi_emxFree_real_T(&pStruct->ErrTemp);
10986 cartesian_waypoi_emxFree_real_T(&pStruct->GradTemp);
10987}
10988
10989static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
10990 *pStruct)
10991{
10992 cartesian_waypoi_emxFree_real_T(&pStruct->ConstraintMatrix);
10993 cartesian_waypoi_emxFree_real_T(&pStruct->ConstraintBound);
10994}
10995
10996static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_wa_T
10997 *pStruct)
10998{
10999 cartesian_waypoi_emxFree_char_T(&pStruct->Type);
11000}
11001
11002static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
11003 *pStruct)
11004{
11005 cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);
11006 emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
11007}
11008
11009static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
11010 *pStruct)
11011{
11012 emxFreeStruct_o_robotics_manip_(&pStruct->Base);
11013}
11014
11015static void emxFreeStruct_robotics_slmani_e(robotics_slmanip_internal_blo_T
11016 *pStruct)
11017{
11018 emxFreeStruct_p_robotics_manip_(&pStruct->TreeInternal);
11019}
11020
11021static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
11022 *pStruct)
11023{
11024 cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);
11025 emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
11026}
11027
11028static void matlabCodegenHandle_matl_evqusn(ros_slros_internal_block_Subs_T *obj)
11029{
11030 if (!obj->matlabCodegenIsDeleted) {
11031 obj->matlabCodegenIsDeleted = true;
11032 }
11033}
11034
11035static void cartesian_way_matlabCodegenHa_j(ros_slros_internal_block_SetP_T *obj)
11036{
11037 if (!obj->matlabCodegenIsDeleted) {
11038 obj->matlabCodegenIsDeleted = true;
11039 }
11040}
11041
11042static void matlabCodegenHandle_matla_evqus(ros_slros_internal_block_Publ_T *obj)
11043{
11044 if (!obj->matlabCodegenIsDeleted) {
11045 obj->matlabCodegenIsDeleted = true;
11046 }
11047}
11048
11049static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian__e_T
11050 *pStruct)
11051{
11052 cartesian_waypoi_emxInit_char_T(&pStruct->Type, 2);
11053 cartesian_waypoi_emxInit_real_T(&pStruct->MotionSubspace, 2);
11054 cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);
11055 cartesian_waypoi_emxInit_real_T(&pStruct->PositionLimitsInternal, 2);
11056 cartesian_waypoi_emxInit_real_T(&pStruct->HomePositionInternal, 1);
11057}
11058
11059static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
11060 *pStruct)
11061{
11062 cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);
11063 emxInitStruct_c_rigidBodyJoint(&pStruct->JointInternal);
11064}
11065
11066static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
11067 *pStruct)
11068{
11069 emxInitStruct_v_robotics_manip_(&pStruct->Base);
11070}
11071
11072static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
11073 *pStruct)
11074{
11075 cartesian_waypoi_emxInit_real_T(&pStruct->Limits, 2);
11076}
11077
11078static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_b_e_T
11079 *pStruct)
11080{
11081 emxInitStruct_y_robotics_manip_(&pStruct->TreeInternal);
11082 emxInitStruct_b_inverseKinemati(&pStruct->IKInternal);
11083}
11084
11085static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
11086 *pStruct)
11087{
11088 cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);
11089}
11090
11091static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
11092 *pStruct)
11093{
11094 emxInitStruct_w_robotics_manip_(&pStruct->Base);
11095}
11096
11097static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
11098 *pStruct)
11099{
11100 cartesian_waypoi_emxInit_char_T(&pStruct->BodyName, 2);
11101 cartesian_waypoi_emxInit_real_T(&pStruct->ErrTemp, 1);
11102 cartesian_waypoi_emxInit_real_T(&pStruct->GradTemp, 1);
11103}
11104
11105static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
11106 *pStruct)
11107{
11108 cartesian_waypoi_emxInit_real_T(&pStruct->ConstraintMatrix, 2);
11109 cartesian_waypoi_emxInit_real_T(&pStruct->ConstraintBound, 1);
11110}
11111
11112static void cartesia_twister_state_vector_e(uint32_T mt[625])
11113{
11114 uint32_T r;
11115 int32_T b_mti;
11116 r = 5489U;
11117 mt[0] = 5489U;
11118 for (b_mti = 0; b_mti < 623; b_mti++) {
11119 r = ((r >> 30U ^ r) * 1812433253U + b_mti) + 1U;
11120 mt[b_mti + 1] = r;
11121 }
11122
11123 mt[624] = 624U;
11124}
11125
11126static void cartesian_wa_eml_rand_mt19937ar(uint32_T state[625])
11127{
11128 memset(&state[0], 0, 625U * sizeof(uint32_T));
11129 cartesia_twister_state_vector_e(state);
11130}
11131
11132static v_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_evqus
11133 (v_robotics_manip_internal_Rig_T *obj)
11134{
11135 v_robotics_manip_internal_Rig_T *b_obj;
11136 int8_T msubspace_data[36];
11137 real_T poslim_data[12];
11138 emxArray_char_T_cartesian_way_T *switch_expression;
11139 boolean_T b_bool;
11140 int32_T b_kstr;
11141 char_T b[8];
11142 char_T b_0[9];
11143 int32_T loop_ub;
11144 int8_T tmp[6];
11145 static const char_T tmp_0[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
11146 'l', 'i', 'n', 'k' };
11147
11148 static const real_T tmp_1[9] = { 0.012583419040406959, -0.00021487638648447484,
11149 -0.00022605919127205462, -0.00021487638648447484, 0.00052369449451288713,
11150 -0.00011525315957400814, -0.00022605919127205462, -0.00011525315957400814,
11151 0.012646079447789898 };
11152
11153 static const real_T tmp_2[36] = { 0.012583419040406959,
11154 -0.00021487638648447484, -0.00022605919127205462, 0.0, -0.00392971169381184,
11155 0.00047022930128152475, -0.00021487638648447484, 0.00052369449451288713,
11156 -0.00011525315957400814, 0.00392971169381184, 0.0, -0.00449464704691423,
11157 -0.00022605919127205462, -0.00011525315957400814, 0.012646079447789898,
11158 -0.00047022930128152475, 0.00449464704691423, 0.0, 0.0, 0.00392971169381184,
11159 -0.00047022930128152475, 0.0785942338762368, 0.0, 0.0, -0.00392971169381184,
11160 0.0, 0.00449464704691423, 0.0, 0.0785942338762368, 0.0,
11161 0.00047022930128152475, -0.00449464704691423, 0.0, 0.0, 0.0,
11162 0.0785942338762368 };
11163
11164 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
11165 1 };
11166
11167 static const char_T tmp_4[15] = { 'w', 'o', 'r', 'l', 'd', '_', 'e', 'd', 'o',
11168 '_', 'j', 'o', 'i', 'n', 't' };
11169
11170 static const char_T tmp_5[5] = { 'f', 'i', 'x', 'e', 'd' };
11171
11172 static const char_T tmp_6[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
11173
11174 static const char_T tmp_7[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
11175
11176 static const real_T tmp_8[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
11177 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
11178
11179 static const real_T tmp_9[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
11180 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
11181
11182 int32_T exitg1;
11183 b_obj = obj;
11184 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
11185 obj->NameInternal->size[0] = 1;
11186 obj->NameInternal->size[1] = 13;
11187 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
11188 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
11189 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
11190 }
11191
11192 obj->ParentIndex = 0.0;
11193 obj->MassInternal = 0.0785942338762368;
11194 obj->CenterOfMassInternal[0] = 0.057188;
11195 obj->CenterOfMassInternal[1] = 0.005983;
11196 obj->CenterOfMassInternal[2] = 0.05;
11197 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
11198 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
11199 }
11200
11201 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
11202 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
11203 }
11204
11205 obj->JointInternal.InTree = false;
11206 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11207 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
11208 }
11209
11210 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11211 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
11212 }
11213
11214 b_kstr = obj->JointInternal.NameInternal->size[0] *
11215 obj->JointInternal.NameInternal->size[1];
11216 obj->JointInternal.NameInternal->size[0] = 1;
11217 obj->JointInternal.NameInternal->size[1] = 15;
11218 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
11219 for (b_kstr = 0; b_kstr < 15; b_kstr++) {
11220 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
11221 }
11222
11223 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
11224 obj->JointInternal.Type->size[0] = 1;
11225 obj->JointInternal.Type->size[1] = 5;
11226 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
11227 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
11228 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
11229 }
11230
11231 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
11232 b_kstr = switch_expression->size[0] * switch_expression->size[1];
11233 switch_expression->size[0] = 1;
11234 switch_expression->size[1] = obj->JointInternal.Type->size[1];
11235 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
11236 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
11237 - 1;
11238 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
11239 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
11240 }
11241
11242 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
11243 b[b_kstr] = tmp_6[b_kstr];
11244 }
11245
11246 b_bool = false;
11247 if (switch_expression->size[1] == 8) {
11248 b_kstr = 1;
11249 do {
11250 exitg1 = 0;
11251 if (b_kstr - 1 < 8) {
11252 loop_ub = b_kstr - 1;
11253 if (switch_expression->data[loop_ub] != b[loop_ub]) {
11254 exitg1 = 1;
11255 } else {
11256 b_kstr++;
11257 }
11258 } else {
11259 b_bool = true;
11260 exitg1 = 1;
11261 }
11262 } while (exitg1 == 0);
11263 }
11264
11265 if (b_bool) {
11266 b_kstr = 0;
11267 } else {
11268 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
11269 b_0[b_kstr] = tmp_7[b_kstr];
11270 }
11271
11272 b_bool = false;
11273 if (switch_expression->size[1] == 9) {
11274 b_kstr = 1;
11275 do {
11276 exitg1 = 0;
11277 if (b_kstr - 1 < 9) {
11278 loop_ub = b_kstr - 1;
11279 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
11280 exitg1 = 1;
11281 } else {
11282 b_kstr++;
11283 }
11284 } else {
11285 b_bool = true;
11286 exitg1 = 1;
11287 }
11288 } while (exitg1 == 0);
11289 }
11290
11291 if (b_bool) {
11292 b_kstr = 1;
11293 } else {
11294 b_kstr = -1;
11295 }
11296 }
11297
11298 cartesian_waypoi_emxFree_char_T(&switch_expression);
11299 switch (b_kstr) {
11300 case 0:
11301 tmp[0] = 0;
11302 tmp[1] = 0;
11303 tmp[2] = 1;
11304 tmp[3] = 0;
11305 tmp[4] = 0;
11306 tmp[5] = 0;
11307 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11308 msubspace_data[b_kstr] = tmp[b_kstr];
11309 }
11310
11311 poslim_data[0] = -3.1415926535897931;
11312 poslim_data[1] = 3.1415926535897931;
11313 obj->JointInternal.VelocityNumber = 1.0;
11314 obj->JointInternal.PositionNumber = 1.0;
11315 obj->JointInternal.JointAxisInternal[0] = 0.0;
11316 obj->JointInternal.JointAxisInternal[1] = 0.0;
11317 obj->JointInternal.JointAxisInternal[2] = 1.0;
11318 break;
11319
11320 case 1:
11321 tmp[0] = 0;
11322 tmp[1] = 0;
11323 tmp[2] = 0;
11324 tmp[3] = 0;
11325 tmp[4] = 0;
11326 tmp[5] = 1;
11327 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11328 msubspace_data[b_kstr] = tmp[b_kstr];
11329 }
11330
11331 poslim_data[0] = -0.5;
11332 poslim_data[1] = 0.5;
11333 obj->JointInternal.VelocityNumber = 1.0;
11334 obj->JointInternal.PositionNumber = 1.0;
11335 obj->JointInternal.JointAxisInternal[0] = 0.0;
11336 obj->JointInternal.JointAxisInternal[1] = 0.0;
11337 obj->JointInternal.JointAxisInternal[2] = 1.0;
11338 break;
11339
11340 default:
11341 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11342 msubspace_data[b_kstr] = 0;
11343 }
11344
11345 poslim_data[0] = 0.0;
11346 poslim_data[1] = 0.0;
11347 obj->JointInternal.VelocityNumber = 0.0;
11348 obj->JointInternal.PositionNumber = 0.0;
11349 obj->JointInternal.JointAxisInternal[0] = 0.0;
11350 obj->JointInternal.JointAxisInternal[1] = 0.0;
11351 obj->JointInternal.JointAxisInternal[2] = 0.0;
11352 break;
11353 }
11354
11355 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
11356 obj->JointInternal.MotionSubspace->size[1];
11357 obj->JointInternal.MotionSubspace->size[0] = 6;
11358 obj->JointInternal.MotionSubspace->size[1] = 1;
11359 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
11360 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11361 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
11362 }
11363
11364 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
11365 obj->JointInternal.PositionLimitsInternal->size[1];
11366 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
11367 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
11368 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
11369 b_kstr);
11370 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
11371 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
11372 }
11373
11374 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
11375 obj->JointInternal.HomePositionInternal->size[0] = 1;
11376 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
11377 b_kstr);
11378 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
11379 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
11380 }
11381
11382 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11383 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_8[b_kstr];
11384 }
11385
11386 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11387 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_9[b_kstr];
11388 }
11389
11390 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
11391 obj->JointInternal.MotionSubspace->size[1];
11392 obj->JointInternal.MotionSubspace->size[0] = 6;
11393 obj->JointInternal.MotionSubspace->size[1] = 1;
11394 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
11395 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11396 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
11397 }
11398
11399 obj->JointInternal.InTree = true;
11400 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
11401 obj->JointInternal.PositionLimitsInternal->size[1];
11402 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
11403 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
11404 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
11405 b_kstr);
11406 obj->JointInternal.PositionLimitsInternal->data[0] = 0.0;
11407 obj->JointInternal.PositionLimitsInternal->data
11408 [obj->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
11409 obj->JointInternal.JointAxisInternal[0] = 0.0;
11410 obj->JointInternal.JointAxisInternal[1] = 0.0;
11411 obj->JointInternal.JointAxisInternal[2] = 0.0;
11412 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
11413 obj->JointInternal.HomePositionInternal->size[0] = 1;
11414 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
11415 b_kstr);
11416 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
11417 return b_obj;
11418}
11419
11420static v_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_evqusn
11421 (v_robotics_manip_internal_Rig_T *obj)
11422{
11423 v_robotics_manip_internal_Rig_T *b_obj;
11424 int8_T msubspace_data[36];
11425 real_T poslim_data[12];
11426 emxArray_char_T_cartesian_way_T *switch_expression;
11427 boolean_T b_bool;
11428 int32_T b_kstr;
11429 char_T b[8];
11430 char_T b_0[9];
11431 int32_T loop_ub;
11432 int8_T tmp[6];
11433 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
11434 '1' };
11435
11436 static const real_T tmp_1[9] = { 0.012559660892485551, 0.00032713982710414,
11437 -1.32683892634271E-6, 0.00032713982710414, 0.00018048007331848145,
11438 9.17416945099368E-5, -1.32683892634271E-6, 9.17416945099368E-5,
11439 0.012672078048055372 };
11440
11441 static const real_T tmp_2[36] = { 0.012559660892485551, 0.00032713982710414,
11442 -1.32683892634271E-6, 0.0, -0.0, 0.0037143634929909515, 0.00032713982710414,
11443 0.00018048007331848145, 9.17416945099368E-5, 0.0, 0.0, 0.0029444543779393356,
11444 -1.32683892634271E-6, 9.17416945099368E-5, 0.012672078048055372,
11445 -0.0037143634929909515, -0.0029444543779393356, 0.0, 0.0, 0.0,
11446 -0.0037143634929909515, 0.0785942338762368, 0.0, 0.0, -0.0, 0.0,
11447 -0.0029444543779393356, 0.0, 0.0785942338762368, 0.0, 0.0037143634929909515,
11448 0.0029444543779393356, 0.0, 0.0, 0.0, 0.0785942338762368 };
11449
11450 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
11451 1 };
11452
11453 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
11454 '_', '1' };
11455
11456 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
11457
11458 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
11459
11460 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
11461 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
11462
11463 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
11464 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
11465
11466 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
11467 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
11468 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
11469
11470 int32_T exitg1;
11471 b_obj = obj;
11472 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
11473 obj->NameInternal->size[0] = 1;
11474 obj->NameInternal->size[1] = 10;
11475 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
11476 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
11477 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
11478 }
11479
11480 obj->ParentIndex = 1.0;
11481 obj->MassInternal = 0.0785942338762368;
11482 obj->CenterOfMassInternal[0] = -0.037464;
11483 obj->CenterOfMassInternal[1] = 0.04726;
11484 obj->CenterOfMassInternal[2] = 0.0;
11485 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
11486 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
11487 }
11488
11489 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
11490 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
11491 }
11492
11493 obj->JointInternal.InTree = false;
11494 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11495 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
11496 }
11497
11498 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11499 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
11500 }
11501
11502 b_kstr = obj->JointInternal.NameInternal->size[0] *
11503 obj->JointInternal.NameInternal->size[1];
11504 obj->JointInternal.NameInternal->size[0] = 1;
11505 obj->JointInternal.NameInternal->size[1] = 11;
11506 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
11507 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
11508 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
11509 }
11510
11511 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
11512 obj->JointInternal.Type->size[0] = 1;
11513 obj->JointInternal.Type->size[1] = 8;
11514 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
11515 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
11516 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
11517 }
11518
11519 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
11520 b_kstr = switch_expression->size[0] * switch_expression->size[1];
11521 switch_expression->size[0] = 1;
11522 switch_expression->size[1] = obj->JointInternal.Type->size[1];
11523 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
11524 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
11525 - 1;
11526 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
11527 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
11528 }
11529
11530 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
11531 b[b_kstr] = tmp_5[b_kstr];
11532 }
11533
11534 b_bool = false;
11535 if (switch_expression->size[1] == 8) {
11536 b_kstr = 1;
11537 do {
11538 exitg1 = 0;
11539 if (b_kstr - 1 < 8) {
11540 loop_ub = b_kstr - 1;
11541 if (switch_expression->data[loop_ub] != b[loop_ub]) {
11542 exitg1 = 1;
11543 } else {
11544 b_kstr++;
11545 }
11546 } else {
11547 b_bool = true;
11548 exitg1 = 1;
11549 }
11550 } while (exitg1 == 0);
11551 }
11552
11553 if (b_bool) {
11554 b_kstr = 0;
11555 } else {
11556 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
11557 b_0[b_kstr] = tmp_6[b_kstr];
11558 }
11559
11560 b_bool = false;
11561 if (switch_expression->size[1] == 9) {
11562 b_kstr = 1;
11563 do {
11564 exitg1 = 0;
11565 if (b_kstr - 1 < 9) {
11566 loop_ub = b_kstr - 1;
11567 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
11568 exitg1 = 1;
11569 } else {
11570 b_kstr++;
11571 }
11572 } else {
11573 b_bool = true;
11574 exitg1 = 1;
11575 }
11576 } while (exitg1 == 0);
11577 }
11578
11579 if (b_bool) {
11580 b_kstr = 1;
11581 } else {
11582 b_kstr = -1;
11583 }
11584 }
11585
11586 cartesian_waypoi_emxFree_char_T(&switch_expression);
11587 switch (b_kstr) {
11588 case 0:
11589 tmp[0] = 0;
11590 tmp[1] = 0;
11591 tmp[2] = 1;
11592 tmp[3] = 0;
11593 tmp[4] = 0;
11594 tmp[5] = 0;
11595 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11596 msubspace_data[b_kstr] = tmp[b_kstr];
11597 }
11598
11599 poslim_data[0] = -3.1415926535897931;
11600 poslim_data[1] = 3.1415926535897931;
11601 obj->JointInternal.VelocityNumber = 1.0;
11602 obj->JointInternal.PositionNumber = 1.0;
11603 obj->JointInternal.JointAxisInternal[0] = 0.0;
11604 obj->JointInternal.JointAxisInternal[1] = 0.0;
11605 obj->JointInternal.JointAxisInternal[2] = 1.0;
11606 break;
11607
11608 case 1:
11609 tmp[0] = 0;
11610 tmp[1] = 0;
11611 tmp[2] = 0;
11612 tmp[3] = 0;
11613 tmp[4] = 0;
11614 tmp[5] = 1;
11615 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11616 msubspace_data[b_kstr] = tmp[b_kstr];
11617 }
11618
11619 poslim_data[0] = -0.5;
11620 poslim_data[1] = 0.5;
11621 obj->JointInternal.VelocityNumber = 1.0;
11622 obj->JointInternal.PositionNumber = 1.0;
11623 obj->JointInternal.JointAxisInternal[0] = 0.0;
11624 obj->JointInternal.JointAxisInternal[1] = 0.0;
11625 obj->JointInternal.JointAxisInternal[2] = 1.0;
11626 break;
11627
11628 default:
11629 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11630 msubspace_data[b_kstr] = 0;
11631 }
11632
11633 poslim_data[0] = 0.0;
11634 poslim_data[1] = 0.0;
11635 obj->JointInternal.VelocityNumber = 0.0;
11636 obj->JointInternal.PositionNumber = 0.0;
11637 obj->JointInternal.JointAxisInternal[0] = 0.0;
11638 obj->JointInternal.JointAxisInternal[1] = 0.0;
11639 obj->JointInternal.JointAxisInternal[2] = 0.0;
11640 break;
11641 }
11642
11643 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
11644 obj->JointInternal.MotionSubspace->size[1];
11645 obj->JointInternal.MotionSubspace->size[0] = 6;
11646 obj->JointInternal.MotionSubspace->size[1] = 1;
11647 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
11648 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11649 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
11650 }
11651
11652 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
11653 obj->JointInternal.PositionLimitsInternal->size[1];
11654 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
11655 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
11656 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
11657 b_kstr);
11658 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
11659 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
11660 }
11661
11662 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
11663 obj->JointInternal.HomePositionInternal->size[0] = 1;
11664 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
11665 b_kstr);
11666 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
11667 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
11668 }
11669
11670 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11671 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
11672 }
11673
11674 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11675 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
11676 }
11677
11678 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
11679 obj->JointInternal.MotionSubspace->size[1];
11680 obj->JointInternal.MotionSubspace->size[0] = 6;
11681 obj->JointInternal.MotionSubspace->size[1] = 1;
11682 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
11683 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11684 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
11685 }
11686
11687 obj->JointInternal.InTree = true;
11688 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
11689 obj->JointInternal.PositionLimitsInternal->size[1];
11690 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
11691 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
11692 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
11693 b_kstr);
11694 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
11695 obj->JointInternal.PositionLimitsInternal->data
11696 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
11697 obj->JointInternal.JointAxisInternal[0] = 0.0;
11698 obj->JointInternal.JointAxisInternal[1] = 0.0;
11699 obj->JointInternal.JointAxisInternal[2] = 1.0;
11700 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
11701 obj->JointInternal.HomePositionInternal->size[0] = 1;
11702 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
11703 b_kstr);
11704 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
11705 return b_obj;
11706}
11707
11708static v_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_evqusng
11709 (v_robotics_manip_internal_Rig_T *obj)
11710{
11711 v_robotics_manip_internal_Rig_T *b_obj;
11712 int8_T msubspace_data[36];
11713 real_T poslim_data[12];
11714 emxArray_char_T_cartesian_way_T *switch_expression;
11715 boolean_T b_bool;
11716 int32_T b_kstr;
11717 char_T b[8];
11718 char_T b_0[9];
11719 int32_T loop_ub;
11720 int8_T tmp[6];
11721 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
11722 '2' };
11723
11724 static const real_T tmp_1[9] = { 0.012452446533447339, 0.00097164648860403489,
11725 -0.0012079400901817208, 0.00097164648860403489, 0.0077631932790618,
11726 0.0060529024261622953, -0.0012079400901817208, 0.0060529024261622953,
11727 0.0051581160550583753 };
11728
11729 static const real_T tmp_2[36] = { 0.012452446533447339, 0.00097164648860403489,
11730 -0.0012079400901817208, 0.0, 0.0036112478581453288, 0.0024995324199659588,
11731 0.00097164648860403489, 0.0077631932790618, 0.0060529024261622953,
11732 -0.0036112478581453288, 0.0, 0.0012907531029494371, -0.0012079400901817208,
11733 0.0060529024261622953, 0.0051581160550583753, -0.0024995324199659588,
11734 -0.0012907531029494371, 0.0, 0.0, -0.0036112478581453288,
11735 -0.0024995324199659588, 0.0785942338762368, 0.0, 0.0, 0.0036112478581453288,
11736 0.0, -0.0012907531029494371, 0.0, 0.0785942338762368, 0.0,
11737 0.0024995324199659588, 0.0012907531029494371, 0.0, 0.0, 0.0,
11738 0.0785942338762368 };
11739
11740 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
11741 1 };
11742
11743 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
11744 '_', '2' };
11745
11746 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
11747
11748 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
11749
11750 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
11751 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
11752 0.0, 0.0, 0.0, 1.0 };
11753
11754 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
11755 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
11756
11757 static const real_T tmp_9[36] = { 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
11758 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
11759 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
11760
11761 int32_T exitg1;
11762 b_obj = obj;
11763 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
11764 obj->NameInternal->size[0] = 1;
11765 obj->NameInternal->size[1] = 10;
11766 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
11767 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
11768 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
11769 }
11770
11771 obj->ParentIndex = 2.0;
11772 obj->MassInternal = 0.0785942338762368;
11773 obj->CenterOfMassInternal[0] = -0.016423;
11774 obj->CenterOfMassInternal[1] = 0.031803;
11775 obj->CenterOfMassInternal[2] = -0.045948;
11776 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
11777 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
11778 }
11779
11780 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
11781 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
11782 }
11783
11784 obj->JointInternal.InTree = false;
11785 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11786 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
11787 }
11788
11789 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11790 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
11791 }
11792
11793 b_kstr = obj->JointInternal.NameInternal->size[0] *
11794 obj->JointInternal.NameInternal->size[1];
11795 obj->JointInternal.NameInternal->size[0] = 1;
11796 obj->JointInternal.NameInternal->size[1] = 11;
11797 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
11798 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
11799 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
11800 }
11801
11802 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
11803 obj->JointInternal.Type->size[0] = 1;
11804 obj->JointInternal.Type->size[1] = 8;
11805 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
11806 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
11807 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
11808 }
11809
11810 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
11811 b_kstr = switch_expression->size[0] * switch_expression->size[1];
11812 switch_expression->size[0] = 1;
11813 switch_expression->size[1] = obj->JointInternal.Type->size[1];
11814 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
11815 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
11816 - 1;
11817 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
11818 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
11819 }
11820
11821 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
11822 b[b_kstr] = tmp_5[b_kstr];
11823 }
11824
11825 b_bool = false;
11826 if (switch_expression->size[1] == 8) {
11827 b_kstr = 1;
11828 do {
11829 exitg1 = 0;
11830 if (b_kstr - 1 < 8) {
11831 loop_ub = b_kstr - 1;
11832 if (switch_expression->data[loop_ub] != b[loop_ub]) {
11833 exitg1 = 1;
11834 } else {
11835 b_kstr++;
11836 }
11837 } else {
11838 b_bool = true;
11839 exitg1 = 1;
11840 }
11841 } while (exitg1 == 0);
11842 }
11843
11844 if (b_bool) {
11845 b_kstr = 0;
11846 } else {
11847 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
11848 b_0[b_kstr] = tmp_6[b_kstr];
11849 }
11850
11851 b_bool = false;
11852 if (switch_expression->size[1] == 9) {
11853 b_kstr = 1;
11854 do {
11855 exitg1 = 0;
11856 if (b_kstr - 1 < 9) {
11857 loop_ub = b_kstr - 1;
11858 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
11859 exitg1 = 1;
11860 } else {
11861 b_kstr++;
11862 }
11863 } else {
11864 b_bool = true;
11865 exitg1 = 1;
11866 }
11867 } while (exitg1 == 0);
11868 }
11869
11870 if (b_bool) {
11871 b_kstr = 1;
11872 } else {
11873 b_kstr = -1;
11874 }
11875 }
11876
11877 cartesian_waypoi_emxFree_char_T(&switch_expression);
11878 switch (b_kstr) {
11879 case 0:
11880 tmp[0] = 0;
11881 tmp[1] = 0;
11882 tmp[2] = 1;
11883 tmp[3] = 0;
11884 tmp[4] = 0;
11885 tmp[5] = 0;
11886 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11887 msubspace_data[b_kstr] = tmp[b_kstr];
11888 }
11889
11890 poslim_data[0] = -3.1415926535897931;
11891 poslim_data[1] = 3.1415926535897931;
11892 obj->JointInternal.VelocityNumber = 1.0;
11893 obj->JointInternal.PositionNumber = 1.0;
11894 obj->JointInternal.JointAxisInternal[0] = 0.0;
11895 obj->JointInternal.JointAxisInternal[1] = 0.0;
11896 obj->JointInternal.JointAxisInternal[2] = 1.0;
11897 break;
11898
11899 case 1:
11900 tmp[0] = 0;
11901 tmp[1] = 0;
11902 tmp[2] = 0;
11903 tmp[3] = 0;
11904 tmp[4] = 0;
11905 tmp[5] = 1;
11906 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11907 msubspace_data[b_kstr] = tmp[b_kstr];
11908 }
11909
11910 poslim_data[0] = -0.5;
11911 poslim_data[1] = 0.5;
11912 obj->JointInternal.VelocityNumber = 1.0;
11913 obj->JointInternal.PositionNumber = 1.0;
11914 obj->JointInternal.JointAxisInternal[0] = 0.0;
11915 obj->JointInternal.JointAxisInternal[1] = 0.0;
11916 obj->JointInternal.JointAxisInternal[2] = 1.0;
11917 break;
11918
11919 default:
11920 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11921 msubspace_data[b_kstr] = 0;
11922 }
11923
11924 poslim_data[0] = 0.0;
11925 poslim_data[1] = 0.0;
11926 obj->JointInternal.VelocityNumber = 0.0;
11927 obj->JointInternal.PositionNumber = 0.0;
11928 obj->JointInternal.JointAxisInternal[0] = 0.0;
11929 obj->JointInternal.JointAxisInternal[1] = 0.0;
11930 obj->JointInternal.JointAxisInternal[2] = 0.0;
11931 break;
11932 }
11933
11934 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
11935 obj->JointInternal.MotionSubspace->size[1];
11936 obj->JointInternal.MotionSubspace->size[0] = 6;
11937 obj->JointInternal.MotionSubspace->size[1] = 1;
11938 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
11939 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11940 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
11941 }
11942
11943 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
11944 obj->JointInternal.PositionLimitsInternal->size[1];
11945 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
11946 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
11947 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
11948 b_kstr);
11949 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
11950 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
11951 }
11952
11953 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
11954 obj->JointInternal.HomePositionInternal->size[0] = 1;
11955 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
11956 b_kstr);
11957 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
11958 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
11959 }
11960
11961 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11962 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
11963 }
11964
11965 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
11966 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
11967 }
11968
11969 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
11970 obj->JointInternal.MotionSubspace->size[1];
11971 obj->JointInternal.MotionSubspace->size[0] = 6;
11972 obj->JointInternal.MotionSubspace->size[1] = 1;
11973 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
11974 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
11975 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
11976 }
11977
11978 obj->JointInternal.InTree = true;
11979 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
11980 obj->JointInternal.PositionLimitsInternal->size[1];
11981 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
11982 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
11983 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
11984 b_kstr);
11985 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
11986 obj->JointInternal.PositionLimitsInternal->data
11987 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
11988 obj->JointInternal.JointAxisInternal[0] = 0.0;
11989 obj->JointInternal.JointAxisInternal[1] = 0.0;
11990 obj->JointInternal.JointAxisInternal[2] = -1.0;
11991 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
11992 obj->JointInternal.HomePositionInternal->size[0] = 1;
11993 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
11994 b_kstr);
11995 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
11996 return b_obj;
11997}
11998
11999static v_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_evqusngv
12000 (v_robotics_manip_internal_Rig_T *obj)
12001{
12002 v_robotics_manip_internal_Rig_T *b_obj;
12003 int8_T msubspace_data[36];
12004 real_T poslim_data[12];
12005 emxArray_char_T_cartesian_way_T *switch_expression;
12006 boolean_T b_bool;
12007 int32_T b_kstr;
12008 char_T b[8];
12009 char_T b_0[9];
12010 int32_T loop_ub;
12011 int8_T tmp[6];
12012 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
12013 '3' };
12014
12015 static const real_T tmp_1[9] = { 0.0123976159829631, 0.00022039108420015264,
12016 2.1332825710116445E-6, 0.00022039108420015264, 0.00014803877895089843,
12017 -9.2077339045129963E-5, 2.1332825710116445E-6, -9.2077339045129963E-5,
12018 0.012477575138803739 };
12019
12020 static const real_T tmp_2[36] = { 0.0123976159829631, 0.00022039108420015264,
12021 2.1332825710116445E-6, 0.0, -2.56217202436532E-5, 0.0010295844637787021,
12022 0.00022039108420015264, 0.00014803877895089843, -9.2077339045129963E-5,
12023 2.56217202436532E-5, 0.0, 0.0024737535112545539, 2.1332825710116445E-6,
12024 -9.2077339045129963E-5, 0.012477575138803739, -0.0010295844637787021,
12025 -0.0024737535112545539, 0.0, 0.0, 2.56217202436532E-5,
12026 -0.0010295844637787021, 0.0785942338762368, 0.0, 0.0, -2.56217202436532E-5,
12027 0.0, -0.0024737535112545539, 0.0, 0.0785942338762368, 0.0,
12028 0.0010295844637787021, 0.0024737535112545539, 0.0, 0.0, 0.0,
12029 0.0785942338762368 };
12030
12031 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
12032 1 };
12033
12034 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
12035 '_', '3' };
12036
12037 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
12038
12039 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
12040
12041 static const real_T tmp_7[16] = { 1.0, 2.0682310711021444E-13,
12042 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
12043 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
12044 1.0 };
12045
12046 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12047 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
12048
12049 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
12050 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
12051 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
12052
12053 int32_T exitg1;
12054 b_obj = obj;
12055 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
12056 obj->NameInternal->size[0] = 1;
12057 obj->NameInternal->size[1] = 10;
12058 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
12059 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
12060 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
12061 }
12062
12063 obj->ParentIndex = 3.0;
12064 obj->MassInternal = 0.0785942338762368;
12065 obj->CenterOfMassInternal[0] = -0.031475;
12066 obj->CenterOfMassInternal[1] = 0.0131;
12067 obj->CenterOfMassInternal[2] = 0.000326;
12068 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
12069 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
12070 }
12071
12072 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
12073 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
12074 }
12075
12076 obj->JointInternal.InTree = false;
12077 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12078 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
12079 }
12080
12081 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12082 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
12083 }
12084
12085 b_kstr = obj->JointInternal.NameInternal->size[0] *
12086 obj->JointInternal.NameInternal->size[1];
12087 obj->JointInternal.NameInternal->size[0] = 1;
12088 obj->JointInternal.NameInternal->size[1] = 11;
12089 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
12090 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
12091 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
12092 }
12093
12094 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
12095 obj->JointInternal.Type->size[0] = 1;
12096 obj->JointInternal.Type->size[1] = 8;
12097 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
12098 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
12099 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
12100 }
12101
12102 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
12103 b_kstr = switch_expression->size[0] * switch_expression->size[1];
12104 switch_expression->size[0] = 1;
12105 switch_expression->size[1] = obj->JointInternal.Type->size[1];
12106 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
12107 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
12108 - 1;
12109 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
12110 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
12111 }
12112
12113 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
12114 b[b_kstr] = tmp_5[b_kstr];
12115 }
12116
12117 b_bool = false;
12118 if (switch_expression->size[1] == 8) {
12119 b_kstr = 1;
12120 do {
12121 exitg1 = 0;
12122 if (b_kstr - 1 < 8) {
12123 loop_ub = b_kstr - 1;
12124 if (switch_expression->data[loop_ub] != b[loop_ub]) {
12125 exitg1 = 1;
12126 } else {
12127 b_kstr++;
12128 }
12129 } else {
12130 b_bool = true;
12131 exitg1 = 1;
12132 }
12133 } while (exitg1 == 0);
12134 }
12135
12136 if (b_bool) {
12137 b_kstr = 0;
12138 } else {
12139 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
12140 b_0[b_kstr] = tmp_6[b_kstr];
12141 }
12142
12143 b_bool = false;
12144 if (switch_expression->size[1] == 9) {
12145 b_kstr = 1;
12146 do {
12147 exitg1 = 0;
12148 if (b_kstr - 1 < 9) {
12149 loop_ub = b_kstr - 1;
12150 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
12151 exitg1 = 1;
12152 } else {
12153 b_kstr++;
12154 }
12155 } else {
12156 b_bool = true;
12157 exitg1 = 1;
12158 }
12159 } while (exitg1 == 0);
12160 }
12161
12162 if (b_bool) {
12163 b_kstr = 1;
12164 } else {
12165 b_kstr = -1;
12166 }
12167 }
12168
12169 cartesian_waypoi_emxFree_char_T(&switch_expression);
12170 switch (b_kstr) {
12171 case 0:
12172 tmp[0] = 0;
12173 tmp[1] = 0;
12174 tmp[2] = 1;
12175 tmp[3] = 0;
12176 tmp[4] = 0;
12177 tmp[5] = 0;
12178 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12179 msubspace_data[b_kstr] = tmp[b_kstr];
12180 }
12181
12182 poslim_data[0] = -3.1415926535897931;
12183 poslim_data[1] = 3.1415926535897931;
12184 obj->JointInternal.VelocityNumber = 1.0;
12185 obj->JointInternal.PositionNumber = 1.0;
12186 obj->JointInternal.JointAxisInternal[0] = 0.0;
12187 obj->JointInternal.JointAxisInternal[1] = 0.0;
12188 obj->JointInternal.JointAxisInternal[2] = 1.0;
12189 break;
12190
12191 case 1:
12192 tmp[0] = 0;
12193 tmp[1] = 0;
12194 tmp[2] = 0;
12195 tmp[3] = 0;
12196 tmp[4] = 0;
12197 tmp[5] = 1;
12198 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12199 msubspace_data[b_kstr] = tmp[b_kstr];
12200 }
12201
12202 poslim_data[0] = -0.5;
12203 poslim_data[1] = 0.5;
12204 obj->JointInternal.VelocityNumber = 1.0;
12205 obj->JointInternal.PositionNumber = 1.0;
12206 obj->JointInternal.JointAxisInternal[0] = 0.0;
12207 obj->JointInternal.JointAxisInternal[1] = 0.0;
12208 obj->JointInternal.JointAxisInternal[2] = 1.0;
12209 break;
12210
12211 default:
12212 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12213 msubspace_data[b_kstr] = 0;
12214 }
12215
12216 poslim_data[0] = 0.0;
12217 poslim_data[1] = 0.0;
12218 obj->JointInternal.VelocityNumber = 0.0;
12219 obj->JointInternal.PositionNumber = 0.0;
12220 obj->JointInternal.JointAxisInternal[0] = 0.0;
12221 obj->JointInternal.JointAxisInternal[1] = 0.0;
12222 obj->JointInternal.JointAxisInternal[2] = 0.0;
12223 break;
12224 }
12225
12226 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
12227 obj->JointInternal.MotionSubspace->size[1];
12228 obj->JointInternal.MotionSubspace->size[0] = 6;
12229 obj->JointInternal.MotionSubspace->size[1] = 1;
12230 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
12231 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12232 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
12233 }
12234
12235 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
12236 obj->JointInternal.PositionLimitsInternal->size[1];
12237 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
12238 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
12239 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
12240 b_kstr);
12241 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
12242 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
12243 }
12244
12245 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
12246 obj->JointInternal.HomePositionInternal->size[0] = 1;
12247 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
12248 b_kstr);
12249 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
12250 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
12251 }
12252
12253 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12254 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
12255 }
12256
12257 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12258 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
12259 }
12260
12261 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
12262 obj->JointInternal.MotionSubspace->size[1];
12263 obj->JointInternal.MotionSubspace->size[0] = 6;
12264 obj->JointInternal.MotionSubspace->size[1] = 1;
12265 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
12266 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12267 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
12268 }
12269
12270 obj->JointInternal.InTree = true;
12271 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
12272 obj->JointInternal.PositionLimitsInternal->size[1];
12273 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
12274 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
12275 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
12276 b_kstr);
12277 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
12278 obj->JointInternal.PositionLimitsInternal->data
12279 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
12280 obj->JointInternal.JointAxisInternal[0] = 0.0;
12281 obj->JointInternal.JointAxisInternal[1] = 0.0;
12282 obj->JointInternal.JointAxisInternal[2] = 1.0;
12283 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
12284 obj->JointInternal.HomePositionInternal->size[0] = 1;
12285 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
12286 b_kstr);
12287 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
12288 return b_obj;
12289}
12290
12291static v_robotics_manip_internal_Rig_T *c_RigidBody_RigidBody_evqusngvo
12292 (v_robotics_manip_internal_Rig_T *obj)
12293{
12294 v_robotics_manip_internal_Rig_T *b_obj;
12295 int8_T msubspace_data[36];
12296 real_T poslim_data[12];
12297 emxArray_char_T_cartesian_way_T *switch_expression;
12298 boolean_T b_bool;
12299 int32_T b_kstr;
12300 char_T b[8];
12301 char_T b_0[9];
12302 int32_T loop_ub;
12303 int8_T tmp[6];
12304 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
12305 '4' };
12306
12307 static const real_T tmp_1[9] = { 0.0123990349928174, -2.7766271471639167E-6,
12308 0.00022466935228286869, -2.7766271471639167E-6, 0.012491487094789458,
12309 9.2330220708293281E-5, 0.00022466935228286869, 9.2330220708293281E-5,
12310 0.00016056153744711284 };
12311
12312 static const real_T tmp_2[36] = { 0.0123990349928174, -2.7766271471639167E-6,
12313 0.00022466935228286869, 0.0, 0.0010818496293063995, 4.275526322867282E-5,
12314 -2.7766271471639167E-6, 0.012491487094789458, 9.2330220708293281E-5,
12315 -0.0010818496293063995, 0.0, -0.0026650518765093138, 0.00022466935228286869,
12316 9.2330220708293281E-5, 0.00016056153744711284, -4.275526322867282E-5,
12317 0.0026650518765093138, 0.0, 0.0, -0.0010818496293063995,
12318 -4.275526322867282E-5, 0.0785942338762368, 0.0, 0.0, 0.0010818496293063995,
12319 0.0, 0.0026650518765093138, 0.0, 0.0785942338762368, 0.0,
12320 4.275526322867282E-5, -0.0026650518765093138, 0.0, 0.0, 0.0,
12321 0.0785942338762368 };
12322
12323 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
12324 1 };
12325
12326 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
12327 '_', '4' };
12328
12329 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
12330
12331 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
12332
12333 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
12334 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
12335 0.0, -0.268, 0.0, 1.0 };
12336
12337 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12338 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
12339
12340 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
12341 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
12342 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
12343
12344 int32_T exitg1;
12345 b_obj = obj;
12346 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
12347 obj->NameInternal->size[0] = 1;
12348 obj->NameInternal->size[1] = 10;
12349 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
12350 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
12351 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
12352 }
12353
12354 obj->ParentIndex = 4.0;
12355 obj->MassInternal = 0.0785942338762368;
12356 obj->CenterOfMassInternal[0] = 0.033909;
12357 obj->CenterOfMassInternal[1] = 0.000544;
12358 obj->CenterOfMassInternal[2] = -0.013765;
12359 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
12360 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
12361 }
12362
12363 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
12364 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
12365 }
12366
12367 obj->JointInternal.InTree = false;
12368 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12369 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
12370 }
12371
12372 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12373 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
12374 }
12375
12376 b_kstr = obj->JointInternal.NameInternal->size[0] *
12377 obj->JointInternal.NameInternal->size[1];
12378 obj->JointInternal.NameInternal->size[0] = 1;
12379 obj->JointInternal.NameInternal->size[1] = 11;
12380 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
12381 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
12382 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
12383 }
12384
12385 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
12386 obj->JointInternal.Type->size[0] = 1;
12387 obj->JointInternal.Type->size[1] = 8;
12388 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
12389 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
12390 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
12391 }
12392
12393 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
12394 b_kstr = switch_expression->size[0] * switch_expression->size[1];
12395 switch_expression->size[0] = 1;
12396 switch_expression->size[1] = obj->JointInternal.Type->size[1];
12397 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
12398 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
12399 - 1;
12400 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
12401 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
12402 }
12403
12404 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
12405 b[b_kstr] = tmp_5[b_kstr];
12406 }
12407
12408 b_bool = false;
12409 if (switch_expression->size[1] == 8) {
12410 b_kstr = 1;
12411 do {
12412 exitg1 = 0;
12413 if (b_kstr - 1 < 8) {
12414 loop_ub = b_kstr - 1;
12415 if (switch_expression->data[loop_ub] != b[loop_ub]) {
12416 exitg1 = 1;
12417 } else {
12418 b_kstr++;
12419 }
12420 } else {
12421 b_bool = true;
12422 exitg1 = 1;
12423 }
12424 } while (exitg1 == 0);
12425 }
12426
12427 if (b_bool) {
12428 b_kstr = 0;
12429 } else {
12430 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
12431 b_0[b_kstr] = tmp_6[b_kstr];
12432 }
12433
12434 b_bool = false;
12435 if (switch_expression->size[1] == 9) {
12436 b_kstr = 1;
12437 do {
12438 exitg1 = 0;
12439 if (b_kstr - 1 < 9) {
12440 loop_ub = b_kstr - 1;
12441 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
12442 exitg1 = 1;
12443 } else {
12444 b_kstr++;
12445 }
12446 } else {
12447 b_bool = true;
12448 exitg1 = 1;
12449 }
12450 } while (exitg1 == 0);
12451 }
12452
12453 if (b_bool) {
12454 b_kstr = 1;
12455 } else {
12456 b_kstr = -1;
12457 }
12458 }
12459
12460 cartesian_waypoi_emxFree_char_T(&switch_expression);
12461 switch (b_kstr) {
12462 case 0:
12463 tmp[0] = 0;
12464 tmp[1] = 0;
12465 tmp[2] = 1;
12466 tmp[3] = 0;
12467 tmp[4] = 0;
12468 tmp[5] = 0;
12469 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12470 msubspace_data[b_kstr] = tmp[b_kstr];
12471 }
12472
12473 poslim_data[0] = -3.1415926535897931;
12474 poslim_data[1] = 3.1415926535897931;
12475 obj->JointInternal.VelocityNumber = 1.0;
12476 obj->JointInternal.PositionNumber = 1.0;
12477 obj->JointInternal.JointAxisInternal[0] = 0.0;
12478 obj->JointInternal.JointAxisInternal[1] = 0.0;
12479 obj->JointInternal.JointAxisInternal[2] = 1.0;
12480 break;
12481
12482 case 1:
12483 tmp[0] = 0;
12484 tmp[1] = 0;
12485 tmp[2] = 0;
12486 tmp[3] = 0;
12487 tmp[4] = 0;
12488 tmp[5] = 1;
12489 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12490 msubspace_data[b_kstr] = tmp[b_kstr];
12491 }
12492
12493 poslim_data[0] = -0.5;
12494 poslim_data[1] = 0.5;
12495 obj->JointInternal.VelocityNumber = 1.0;
12496 obj->JointInternal.PositionNumber = 1.0;
12497 obj->JointInternal.JointAxisInternal[0] = 0.0;
12498 obj->JointInternal.JointAxisInternal[1] = 0.0;
12499 obj->JointInternal.JointAxisInternal[2] = 1.0;
12500 break;
12501
12502 default:
12503 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12504 msubspace_data[b_kstr] = 0;
12505 }
12506
12507 poslim_data[0] = 0.0;
12508 poslim_data[1] = 0.0;
12509 obj->JointInternal.VelocityNumber = 0.0;
12510 obj->JointInternal.PositionNumber = 0.0;
12511 obj->JointInternal.JointAxisInternal[0] = 0.0;
12512 obj->JointInternal.JointAxisInternal[1] = 0.0;
12513 obj->JointInternal.JointAxisInternal[2] = 0.0;
12514 break;
12515 }
12516
12517 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
12518 obj->JointInternal.MotionSubspace->size[1];
12519 obj->JointInternal.MotionSubspace->size[0] = 6;
12520 obj->JointInternal.MotionSubspace->size[1] = 1;
12521 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
12522 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12523 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
12524 }
12525
12526 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
12527 obj->JointInternal.PositionLimitsInternal->size[1];
12528 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
12529 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
12530 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
12531 b_kstr);
12532 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
12533 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
12534 }
12535
12536 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
12537 obj->JointInternal.HomePositionInternal->size[0] = 1;
12538 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
12539 b_kstr);
12540 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
12541 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
12542 }
12543
12544 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12545 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
12546 }
12547
12548 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12549 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
12550 }
12551
12552 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
12553 obj->JointInternal.MotionSubspace->size[1];
12554 obj->JointInternal.MotionSubspace->size[0] = 6;
12555 obj->JointInternal.MotionSubspace->size[1] = 1;
12556 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
12557 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12558 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
12559 }
12560
12561 obj->JointInternal.InTree = true;
12562 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
12563 obj->JointInternal.PositionLimitsInternal->size[1];
12564 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
12565 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
12566 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
12567 b_kstr);
12568 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
12569 obj->JointInternal.PositionLimitsInternal->data
12570 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
12571 obj->JointInternal.JointAxisInternal[0] = 0.0;
12572 obj->JointInternal.JointAxisInternal[1] = 0.0;
12573 obj->JointInternal.JointAxisInternal[2] = 1.0;
12574 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
12575 obj->JointInternal.HomePositionInternal->size[0] = 1;
12576 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
12577 b_kstr);
12578 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
12579 return b_obj;
12580}
12581
12582static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0
12583 (v_robotics_manip_internal_Rig_T *obj)
12584{
12585 v_robotics_manip_internal_Rig_T *b_obj;
12586 int8_T msubspace_data[36];
12587 real_T poslim_data[12];
12588 emxArray_char_T_cartesian_way_T *switch_expression;
12589 boolean_T b_bool;
12590 int32_T b_kstr;
12591 char_T b[8];
12592 char_T b_0[9];
12593 int32_T loop_ub;
12594 int8_T tmp[6];
12595 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
12596 '5' };
12597
12598 static const real_T tmp_1[9] = { 0.012440329403329006, 2.388185677857016E-6,
12599 0.00012602126519218373, 2.388185677857016E-6, 0.012510746127660349,
12600 -9.077919321137075E-5, 0.00012602126519218373, -9.077919321137075E-5,
12601 0.00013851261456175015 };
12602
12603 static const real_T tmp_2[36] = { 0.012440329403329006, 2.388185677857016E-6,
12604 0.00012602126519218373, 0.0, -0.0021015312196166957, -3.5996159115316455E-5,
12605 2.388185677857016E-6, 0.012510746127660349, -9.077919321137075E-5,
12606 0.0021015312196166957, 0.0, -0.0023173509858408423, 0.00012602126519218373,
12607 -9.077919321137075E-5, 0.00013851261456175015, 3.5996159115316455E-5,
12608 0.0023173509858408423, 0.0, 0.0, 0.0021015312196166957,
12609 3.5996159115316455E-5, 0.0785942338762368, 0.0, 0.0, -0.0021015312196166957,
12610 0.0, 0.0023173509858408423, 0.0, 0.0785942338762368, 0.0,
12611 -3.5996159115316455E-5, -0.0023173509858408423, 0.0, 0.0, 0.0,
12612 0.0785942338762368 };
12613
12614 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
12615 1 };
12616
12617 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
12618 '_', '5' };
12619
12620 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
12621
12622 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
12623
12624 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12625 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
12626
12627 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12628 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
12629
12630 static const real_T tmp_9[36] = { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
12631 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
12632 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
12633
12634 int32_T exitg1;
12635 b_obj = obj;
12636 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
12637 obj->NameInternal->size[0] = 1;
12638 obj->NameInternal->size[1] = 10;
12639 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
12640 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
12641 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
12642 }
12643
12644 obj->ParentIndex = 5.0;
12645 obj->MassInternal = 0.0785942338762368;
12646 obj->CenterOfMassInternal[0] = 0.029485;
12647 obj->CenterOfMassInternal[1] = -0.000458;
12648 obj->CenterOfMassInternal[2] = 0.026739;
12649 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
12650 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
12651 }
12652
12653 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
12654 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
12655 }
12656
12657 obj->JointInternal.InTree = false;
12658 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12659 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
12660 }
12661
12662 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12663 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
12664 }
12665
12666 b_kstr = obj->JointInternal.NameInternal->size[0] *
12667 obj->JointInternal.NameInternal->size[1];
12668 obj->JointInternal.NameInternal->size[0] = 1;
12669 obj->JointInternal.NameInternal->size[1] = 11;
12670 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
12671 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
12672 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
12673 }
12674
12675 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
12676 obj->JointInternal.Type->size[0] = 1;
12677 obj->JointInternal.Type->size[1] = 8;
12678 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
12679 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
12680 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
12681 }
12682
12683 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
12684 b_kstr = switch_expression->size[0] * switch_expression->size[1];
12685 switch_expression->size[0] = 1;
12686 switch_expression->size[1] = obj->JointInternal.Type->size[1];
12687 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
12688 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
12689 - 1;
12690 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
12691 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
12692 }
12693
12694 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
12695 b[b_kstr] = tmp_5[b_kstr];
12696 }
12697
12698 b_bool = false;
12699 if (switch_expression->size[1] == 8) {
12700 b_kstr = 1;
12701 do {
12702 exitg1 = 0;
12703 if (b_kstr - 1 < 8) {
12704 loop_ub = b_kstr - 1;
12705 if (switch_expression->data[loop_ub] != b[loop_ub]) {
12706 exitg1 = 1;
12707 } else {
12708 b_kstr++;
12709 }
12710 } else {
12711 b_bool = true;
12712 exitg1 = 1;
12713 }
12714 } while (exitg1 == 0);
12715 }
12716
12717 if (b_bool) {
12718 b_kstr = 0;
12719 } else {
12720 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
12721 b_0[b_kstr] = tmp_6[b_kstr];
12722 }
12723
12724 b_bool = false;
12725 if (switch_expression->size[1] == 9) {
12726 b_kstr = 1;
12727 do {
12728 exitg1 = 0;
12729 if (b_kstr - 1 < 9) {
12730 loop_ub = b_kstr - 1;
12731 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
12732 exitg1 = 1;
12733 } else {
12734 b_kstr++;
12735 }
12736 } else {
12737 b_bool = true;
12738 exitg1 = 1;
12739 }
12740 } while (exitg1 == 0);
12741 }
12742
12743 if (b_bool) {
12744 b_kstr = 1;
12745 } else {
12746 b_kstr = -1;
12747 }
12748 }
12749
12750 cartesian_waypoi_emxFree_char_T(&switch_expression);
12751 switch (b_kstr) {
12752 case 0:
12753 tmp[0] = 0;
12754 tmp[1] = 0;
12755 tmp[2] = 1;
12756 tmp[3] = 0;
12757 tmp[4] = 0;
12758 tmp[5] = 0;
12759 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12760 msubspace_data[b_kstr] = tmp[b_kstr];
12761 }
12762
12763 poslim_data[0] = -3.1415926535897931;
12764 poslim_data[1] = 3.1415926535897931;
12765 obj->JointInternal.VelocityNumber = 1.0;
12766 obj->JointInternal.PositionNumber = 1.0;
12767 obj->JointInternal.JointAxisInternal[0] = 0.0;
12768 obj->JointInternal.JointAxisInternal[1] = 0.0;
12769 obj->JointInternal.JointAxisInternal[2] = 1.0;
12770 break;
12771
12772 case 1:
12773 tmp[0] = 0;
12774 tmp[1] = 0;
12775 tmp[2] = 0;
12776 tmp[3] = 0;
12777 tmp[4] = 0;
12778 tmp[5] = 1;
12779 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12780 msubspace_data[b_kstr] = tmp[b_kstr];
12781 }
12782
12783 poslim_data[0] = -0.5;
12784 poslim_data[1] = 0.5;
12785 obj->JointInternal.VelocityNumber = 1.0;
12786 obj->JointInternal.PositionNumber = 1.0;
12787 obj->JointInternal.JointAxisInternal[0] = 0.0;
12788 obj->JointInternal.JointAxisInternal[1] = 0.0;
12789 obj->JointInternal.JointAxisInternal[2] = 1.0;
12790 break;
12791
12792 default:
12793 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12794 msubspace_data[b_kstr] = 0;
12795 }
12796
12797 poslim_data[0] = 0.0;
12798 poslim_data[1] = 0.0;
12799 obj->JointInternal.VelocityNumber = 0.0;
12800 obj->JointInternal.PositionNumber = 0.0;
12801 obj->JointInternal.JointAxisInternal[0] = 0.0;
12802 obj->JointInternal.JointAxisInternal[1] = 0.0;
12803 obj->JointInternal.JointAxisInternal[2] = 0.0;
12804 break;
12805 }
12806
12807 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
12808 obj->JointInternal.MotionSubspace->size[1];
12809 obj->JointInternal.MotionSubspace->size[0] = 6;
12810 obj->JointInternal.MotionSubspace->size[1] = 1;
12811 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
12812 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12813 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
12814 }
12815
12816 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
12817 obj->JointInternal.PositionLimitsInternal->size[1];
12818 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
12819 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
12820 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
12821 b_kstr);
12822 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
12823 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
12824 }
12825
12826 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
12827 obj->JointInternal.HomePositionInternal->size[0] = 1;
12828 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
12829 b_kstr);
12830 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
12831 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
12832 }
12833
12834 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12835 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
12836 }
12837
12838 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12839 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
12840 }
12841
12842 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
12843 obj->JointInternal.MotionSubspace->size[1];
12844 obj->JointInternal.MotionSubspace->size[0] = 6;
12845 obj->JointInternal.MotionSubspace->size[1] = 1;
12846 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
12847 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12848 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
12849 }
12850
12851 obj->JointInternal.InTree = true;
12852 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
12853 obj->JointInternal.PositionLimitsInternal->size[1];
12854 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
12855 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
12856 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
12857 b_kstr);
12858 obj->JointInternal.PositionLimitsInternal->data[0] = -1.79768912955;
12859 obj->JointInternal.PositionLimitsInternal->data
12860 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.79768912955;
12861 obj->JointInternal.JointAxisInternal[0] = 0.0;
12862 obj->JointInternal.JointAxisInternal[1] = 1.0;
12863 obj->JointInternal.JointAxisInternal[2] = 0.0;
12864 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
12865 obj->JointInternal.HomePositionInternal->size[0] = 1;
12866 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
12867 b_kstr);
12868 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
12869 return b_obj;
12870}
12871
12872static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0a
12873 (v_robotics_manip_internal_Rig_T *obj)
12874{
12875 v_robotics_manip_internal_Rig_T *b_obj;
12876 int8_T msubspace_data[36];
12877 real_T poslim_data[12];
12878 emxArray_char_T_cartesian_way_T *switch_expression;
12879 boolean_T b_bool;
12880 int32_T b_kstr;
12881 char_T b[8];
12882 char_T b_0[9];
12883 int32_T loop_ub;
12884 int8_T tmp[6];
12885 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
12886 '6' };
12887
12888 static const real_T tmp_1[9] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
12889 5.3285284099072352E-9, 2.0153545938371486E-9, 1.4574493611914028E-5,
12890 1.6742291194075022E-9, 5.3285284099072352E-9, 1.6742291194075022E-9,
12891 1.006193323459457E-5 };
12892
12893 static const real_T tmp_2[36] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
12894 5.3285284099072352E-9, 0.0, -1.6782149839359722E-7, -0.00026087851925284686,
12895 2.0153545938371486E-9, 1.4574493611914028E-5, 1.6742291194075022E-9,
12896 1.6782149839359722E-7, 0.0, -1.957917481258634E-7, 5.3285284099072352E-9,
12897 1.6742291194075022E-9, 1.006193323459457E-5, 0.00026087851925284686,
12898 1.957917481258634E-7, 0.0, 0.0, 1.6782149839359722E-7,
12899 0.00026087851925284686, 0.0279702497322662, 0.0, 0.0, -1.6782149839359722E-7,
12900 0.0, 1.957917481258634E-7, 0.0, 0.0279702497322662, 0.0,
12901 -0.00026087851925284686, -1.957917481258634E-7, 0.0, 0.0, 0.0,
12902 0.0279702497322662 };
12903
12904 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
12905 1 };
12906
12907 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
12908 '_', '6' };
12909
12910 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
12911
12912 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
12913
12914 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12915 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
12916
12917 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12918 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
12919
12920 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
12921 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
12922 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
12923
12924 int32_T exitg1;
12925 b_obj = obj;
12926 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
12927 obj->NameInternal->size[0] = 1;
12928 obj->NameInternal->size[1] = 10;
12929 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
12930 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
12931 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
12932 }
12933
12934 obj->ParentIndex = 6.0;
12935 obj->MassInternal = 0.0279702497322662;
12936 obj->CenterOfMassInternal[0] = 7.0E-6;
12937 obj->CenterOfMassInternal[1] = -0.009327;
12938 obj->CenterOfMassInternal[2] = 6.0E-6;
12939 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
12940 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
12941 }
12942
12943 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
12944 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
12945 }
12946
12947 obj->JointInternal.InTree = false;
12948 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12949 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
12950 }
12951
12952 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12953 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
12954 }
12955
12956 b_kstr = obj->JointInternal.NameInternal->size[0] *
12957 obj->JointInternal.NameInternal->size[1];
12958 obj->JointInternal.NameInternal->size[0] = 1;
12959 obj->JointInternal.NameInternal->size[1] = 11;
12960 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
12961 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
12962 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
12963 }
12964
12965 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
12966 obj->JointInternal.Type->size[0] = 1;
12967 obj->JointInternal.Type->size[1] = 8;
12968 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
12969 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
12970 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
12971 }
12972
12973 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
12974 b_kstr = switch_expression->size[0] * switch_expression->size[1];
12975 switch_expression->size[0] = 1;
12976 switch_expression->size[1] = obj->JointInternal.Type->size[1];
12977 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
12978 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
12979 - 1;
12980 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
12981 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
12982 }
12983
12984 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
12985 b[b_kstr] = tmp_5[b_kstr];
12986 }
12987
12988 b_bool = false;
12989 if (switch_expression->size[1] == 8) {
12990 b_kstr = 1;
12991 do {
12992 exitg1 = 0;
12993 if (b_kstr - 1 < 8) {
12994 loop_ub = b_kstr - 1;
12995 if (switch_expression->data[loop_ub] != b[loop_ub]) {
12996 exitg1 = 1;
12997 } else {
12998 b_kstr++;
12999 }
13000 } else {
13001 b_bool = true;
13002 exitg1 = 1;
13003 }
13004 } while (exitg1 == 0);
13005 }
13006
13007 if (b_bool) {
13008 b_kstr = 0;
13009 } else {
13010 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13011 b_0[b_kstr] = tmp_6[b_kstr];
13012 }
13013
13014 b_bool = false;
13015 if (switch_expression->size[1] == 9) {
13016 b_kstr = 1;
13017 do {
13018 exitg1 = 0;
13019 if (b_kstr - 1 < 9) {
13020 loop_ub = b_kstr - 1;
13021 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
13022 exitg1 = 1;
13023 } else {
13024 b_kstr++;
13025 }
13026 } else {
13027 b_bool = true;
13028 exitg1 = 1;
13029 }
13030 } while (exitg1 == 0);
13031 }
13032
13033 if (b_bool) {
13034 b_kstr = 1;
13035 } else {
13036 b_kstr = -1;
13037 }
13038 }
13039
13040 cartesian_waypoi_emxFree_char_T(&switch_expression);
13041 switch (b_kstr) {
13042 case 0:
13043 tmp[0] = 0;
13044 tmp[1] = 0;
13045 tmp[2] = 1;
13046 tmp[3] = 0;
13047 tmp[4] = 0;
13048 tmp[5] = 0;
13049 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13050 msubspace_data[b_kstr] = tmp[b_kstr];
13051 }
13052
13053 poslim_data[0] = -3.1415926535897931;
13054 poslim_data[1] = 3.1415926535897931;
13055 obj->JointInternal.VelocityNumber = 1.0;
13056 obj->JointInternal.PositionNumber = 1.0;
13057 obj->JointInternal.JointAxisInternal[0] = 0.0;
13058 obj->JointInternal.JointAxisInternal[1] = 0.0;
13059 obj->JointInternal.JointAxisInternal[2] = 1.0;
13060 break;
13061
13062 case 1:
13063 tmp[0] = 0;
13064 tmp[1] = 0;
13065 tmp[2] = 0;
13066 tmp[3] = 0;
13067 tmp[4] = 0;
13068 tmp[5] = 1;
13069 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13070 msubspace_data[b_kstr] = tmp[b_kstr];
13071 }
13072
13073 poslim_data[0] = -0.5;
13074 poslim_data[1] = 0.5;
13075 obj->JointInternal.VelocityNumber = 1.0;
13076 obj->JointInternal.PositionNumber = 1.0;
13077 obj->JointInternal.JointAxisInternal[0] = 0.0;
13078 obj->JointInternal.JointAxisInternal[1] = 0.0;
13079 obj->JointInternal.JointAxisInternal[2] = 1.0;
13080 break;
13081
13082 default:
13083 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13084 msubspace_data[b_kstr] = 0;
13085 }
13086
13087 poslim_data[0] = 0.0;
13088 poslim_data[1] = 0.0;
13089 obj->JointInternal.VelocityNumber = 0.0;
13090 obj->JointInternal.PositionNumber = 0.0;
13091 obj->JointInternal.JointAxisInternal[0] = 0.0;
13092 obj->JointInternal.JointAxisInternal[1] = 0.0;
13093 obj->JointInternal.JointAxisInternal[2] = 0.0;
13094 break;
13095 }
13096
13097 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13098 obj->JointInternal.MotionSubspace->size[1];
13099 obj->JointInternal.MotionSubspace->size[0] = 6;
13100 obj->JointInternal.MotionSubspace->size[1] = 1;
13101 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13102 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13103 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
13104 }
13105
13106 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13107 obj->JointInternal.PositionLimitsInternal->size[1];
13108 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13109 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13110 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13111 b_kstr);
13112 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
13113 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
13114 }
13115
13116 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13117 obj->JointInternal.HomePositionInternal->size[0] = 1;
13118 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13119 b_kstr);
13120 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
13121 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13122 }
13123
13124 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13125 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
13126 }
13127
13128 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13129 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
13130 }
13131
13132 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13133 obj->JointInternal.MotionSubspace->size[1];
13134 obj->JointInternal.MotionSubspace->size[0] = 6;
13135 obj->JointInternal.MotionSubspace->size[1] = 1;
13136 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13137 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13138 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
13139 }
13140
13141 obj->JointInternal.InTree = true;
13142 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13143 obj->JointInternal.PositionLimitsInternal->size[1];
13144 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13145 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13146 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13147 b_kstr);
13148 obj->JointInternal.PositionLimitsInternal->data[0] = -4.71238898038;
13149 obj->JointInternal.PositionLimitsInternal->data
13150 [obj->JointInternal.PositionLimitsInternal->size[0]] = 4.71238898038;
13151 obj->JointInternal.JointAxisInternal[0] = 0.0;
13152 obj->JointInternal.JointAxisInternal[1] = 0.0;
13153 obj->JointInternal.JointAxisInternal[2] = 1.0;
13154 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13155 obj->JointInternal.HomePositionInternal->size[0] = 1;
13156 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13157 b_kstr);
13158 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13159 return b_obj;
13160}
13161
13162static y_robotics_manip_internal_Rig_T *c_RigidBodyTree_RigidBodyTree_e
13163 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
13164 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
13165 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
13166 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
13167 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
13168 v_robotics_manip_internal_Rig_T *iobj_7)
13169{
13170 y_robotics_manip_internal_Rig_T *b_obj;
13171 v_robotics_manip_internal_Rig_T *obj_0;
13172 int8_T msubspace_data[36];
13173 real_T poslim_data[12];
13174 int8_T b_I[9];
13175 emxArray_char_T_cartesian_way_T *switch_expression;
13176 boolean_T b_bool;
13177 int32_T b_kstr;
13178 char_T b[8];
13179 char_T b_0[9];
13180 int32_T loop_ub;
13181 int8_T tmp[6];
13182 static const char_T tmp_0[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
13183 'e', 'e' };
13184
13185 static const real_T tmp_1[36] = { 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0,
13186 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0,
13187 -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0 };
13188
13189 static const int8_T tmp_2[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
13190 1 };
13191
13192 static const char_T tmp_3[12] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
13193 '_', 'e', 'e' };
13194
13195 static const char_T tmp_4[5] = { 'f', 'i', 'x', 'e', 'd' };
13196
13197 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
13198
13199 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
13200
13201 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
13202 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
13203
13204 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
13205 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
13206
13207 static const char_T tmp_9[5] = { 'w', 'o', 'r', 'l', 'd' };
13208
13209 static const char_T tmp_a[9] = { 'w', 'o', 'r', 'l', 'd', '_', 'j', 'n', 't' };
13210
13211 int32_T exitg1;
13212 b_obj = obj;
13213 obj->Bodies[0] = carte_RigidBody_RigidBody_evqus(iobj_7);
13214 obj->Bodies[0]->Index = 1.0;
13215 obj->Bodies[1] = cart_RigidBody_RigidBody_evqusn(iobj_0);
13216 obj->Bodies[1]->Index = 2.0;
13217 obj->Bodies[2] = car_RigidBody_RigidBody_evqusng(iobj_1);
13218 obj->Bodies[2]->Index = 3.0;
13219 obj->Bodies[3] = ca_RigidBody_RigidBody_evqusngv(iobj_2);
13220 obj->Bodies[3]->Index = 4.0;
13221 obj->Bodies[4] = c_RigidBody_RigidBody_evqusngvo(iobj_3);
13222 obj->Bodies[4]->Index = 5.0;
13223 obj->Bodies[5] = RigidBody_RigidBody_evqusngvo0(iobj_4);
13224 obj->Bodies[5]->Index = 6.0;
13225 obj->Bodies[6] = RigidBody_RigidBody_evqusngvo0a(iobj_5);
13226 obj->Bodies[6]->Index = 7.0;
13227 b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];
13228 iobj_6->NameInternal->size[0] = 1;
13229 iobj_6->NameInternal->size[1] = 11;
13230 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, b_kstr);
13231 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
13232 iobj_6->NameInternal->data[b_kstr] = tmp_0[b_kstr];
13233 }
13234
13235 iobj_6->ParentIndex = 7.0;
13236 iobj_6->MassInternal = 0.0;
13237 iobj_6->CenterOfMassInternal[0] = 0.0;
13238 iobj_6->CenterOfMassInternal[1] = 0.0;
13239 iobj_6->CenterOfMassInternal[2] = 0.0;
13240 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13241 iobj_6->InertiaInternal[b_kstr] = 0.0;
13242 }
13243
13244 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
13245 iobj_6->SpatialInertia[b_kstr] = tmp_1[b_kstr];
13246 }
13247
13248 iobj_6->JointInternal.InTree = false;
13249 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13250 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
13251 }
13252
13253 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13254 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
13255 }
13256
13257 b_kstr = iobj_6->JointInternal.NameInternal->size[0] *
13258 iobj_6->JointInternal.NameInternal->size[1];
13259 iobj_6->JointInternal.NameInternal->size[0] = 1;
13260 iobj_6->JointInternal.NameInternal->size[1] = 12;
13261 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.NameInternal, b_kstr);
13262 for (b_kstr = 0; b_kstr < 12; b_kstr++) {
13263 iobj_6->JointInternal.NameInternal->data[b_kstr] = tmp_3[b_kstr];
13264 }
13265
13266 b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->
13267 JointInternal.Type->size[1];
13268 iobj_6->JointInternal.Type->size[0] = 1;
13269 iobj_6->JointInternal.Type->size[1] = 5;
13270 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.Type, b_kstr);
13271 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
13272 iobj_6->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
13273 }
13274
13275 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
13276 b_kstr = switch_expression->size[0] * switch_expression->size[1];
13277 switch_expression->size[0] = 1;
13278 switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];
13279 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
13280 loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->
13281 JointInternal.Type->size[1] - 1;
13282 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
13283 switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];
13284 }
13285
13286 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13287 b[b_kstr] = tmp_5[b_kstr];
13288 }
13289
13290 b_bool = false;
13291 if (switch_expression->size[1] == 8) {
13292 b_kstr = 1;
13293 do {
13294 exitg1 = 0;
13295 if (b_kstr - 1 < 8) {
13296 loop_ub = b_kstr - 1;
13297 if (switch_expression->data[loop_ub] != b[loop_ub]) {
13298 exitg1 = 1;
13299 } else {
13300 b_kstr++;
13301 }
13302 } else {
13303 b_bool = true;
13304 exitg1 = 1;
13305 }
13306 } while (exitg1 == 0);
13307 }
13308
13309 if (b_bool) {
13310 b_kstr = 0;
13311 } else {
13312 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13313 b_0[b_kstr] = tmp_6[b_kstr];
13314 }
13315
13316 b_bool = false;
13317 if (switch_expression->size[1] == 9) {
13318 b_kstr = 1;
13319 do {
13320 exitg1 = 0;
13321 if (b_kstr - 1 < 9) {
13322 loop_ub = b_kstr - 1;
13323 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
13324 exitg1 = 1;
13325 } else {
13326 b_kstr++;
13327 }
13328 } else {
13329 b_bool = true;
13330 exitg1 = 1;
13331 }
13332 } while (exitg1 == 0);
13333 }
13334
13335 if (b_bool) {
13336 b_kstr = 1;
13337 } else {
13338 b_kstr = -1;
13339 }
13340 }
13341
13342 switch (b_kstr) {
13343 case 0:
13344 tmp[0] = 0;
13345 tmp[1] = 0;
13346 tmp[2] = 1;
13347 tmp[3] = 0;
13348 tmp[4] = 0;
13349 tmp[5] = 0;
13350 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13351 msubspace_data[b_kstr] = tmp[b_kstr];
13352 }
13353
13354 poslim_data[0] = -3.1415926535897931;
13355 poslim_data[1] = 3.1415926535897931;
13356 iobj_6->JointInternal.VelocityNumber = 1.0;
13357 iobj_6->JointInternal.PositionNumber = 1.0;
13358 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
13359 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
13360 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
13361 break;
13362
13363 case 1:
13364 tmp[0] = 0;
13365 tmp[1] = 0;
13366 tmp[2] = 0;
13367 tmp[3] = 0;
13368 tmp[4] = 0;
13369 tmp[5] = 1;
13370 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13371 msubspace_data[b_kstr] = tmp[b_kstr];
13372 }
13373
13374 poslim_data[0] = -0.5;
13375 poslim_data[1] = 0.5;
13376 iobj_6->JointInternal.VelocityNumber = 1.0;
13377 iobj_6->JointInternal.PositionNumber = 1.0;
13378 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
13379 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
13380 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
13381 break;
13382
13383 default:
13384 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13385 msubspace_data[b_kstr] = 0;
13386 }
13387
13388 poslim_data[0] = 0.0;
13389 poslim_data[1] = 0.0;
13390 iobj_6->JointInternal.VelocityNumber = 0.0;
13391 iobj_6->JointInternal.PositionNumber = 0.0;
13392 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
13393 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
13394 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
13395 break;
13396 }
13397
13398 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
13399 iobj_6->JointInternal.MotionSubspace->size[1];
13400 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
13401 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
13402 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);
13403 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13404 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
13405 }
13406
13407 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
13408 iobj_6->JointInternal.PositionLimitsInternal->size[1];
13409 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
13410 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
13411 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
13412 b_kstr);
13413 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
13414 iobj_6->JointInternal.PositionLimitsInternal->data[b_kstr] =
13415 poslim_data[b_kstr];
13416 }
13417
13418 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
13419 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
13420 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
13421 b_kstr);
13422 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
13423 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
13424 }
13425
13426 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13427 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
13428 }
13429
13430 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13431 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
13432 }
13433
13434 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
13435 iobj_6->JointInternal.MotionSubspace->size[1];
13436 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
13437 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
13438 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);
13439 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13440 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
13441 }
13442
13443 iobj_6->JointInternal.InTree = true;
13444 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
13445 iobj_6->JointInternal.PositionLimitsInternal->size[1];
13446 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
13447 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
13448 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
13449 b_kstr);
13450 iobj_6->JointInternal.PositionLimitsInternal->data[0] = 0.0;
13451 iobj_6->JointInternal.PositionLimitsInternal->data
13452 [iobj_6->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
13453 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
13454 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
13455 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
13456 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
13457 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
13458 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
13459 b_kstr);
13460 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
13461 obj->Bodies[7] = iobj_6;
13462 obj->Bodies[7]->Index = 8.0;
13463 obj->NumBodies = 8.0;
13464 obj->Gravity[0] = 0.0;
13465 obj->Gravity[1] = 0.0;
13466 obj->Gravity[2] = 0.0;
13467 obj_0 = &obj->Base;
13468 b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];
13469 obj->Base.NameInternal->size[0] = 1;
13470 obj->Base.NameInternal->size[1] = 5;
13471 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, b_kstr);
13472 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
13473 obj->Base.NameInternal->data[b_kstr] = tmp_9[b_kstr];
13474 }
13475
13476 obj->Base.JointInternal.InTree = false;
13477 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13478 obj_0->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
13479 }
13480
13481 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13482 obj_0->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
13483 }
13484
13485 b_kstr = obj->Base.JointInternal.NameInternal->size[0] *
13486 obj->Base.JointInternal.NameInternal->size[1];
13487 obj->Base.JointInternal.NameInternal->size[0] = 1;
13488 obj->Base.JointInternal.NameInternal->size[1] = 9;
13489 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.NameInternal, b_kstr);
13490 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13491 obj_0->JointInternal.NameInternal->data[b_kstr] = tmp_a[b_kstr];
13492 }
13493
13494 b_kstr = obj->Base.JointInternal.Type->size[0] * obj->
13495 Base.JointInternal.Type->size[1];
13496 obj->Base.JointInternal.Type->size[0] = 1;
13497 obj->Base.JointInternal.Type->size[1] = 5;
13498 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.Type, b_kstr);
13499 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
13500 obj_0->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
13501 }
13502
13503 b_kstr = switch_expression->size[0] * switch_expression->size[1];
13504 switch_expression->size[0] = 1;
13505 switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];
13506 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
13507 loop_ub = obj->Base.JointInternal.Type->size[0] * obj->
13508 Base.JointInternal.Type->size[1] - 1;
13509 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
13510 switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];
13511 }
13512
13513 b_bool = false;
13514 if (switch_expression->size[1] == 8) {
13515 b_kstr = 1;
13516 do {
13517 exitg1 = 0;
13518 if (b_kstr - 1 < 8) {
13519 loop_ub = b_kstr - 1;
13520 if (switch_expression->data[loop_ub] != b[loop_ub]) {
13521 exitg1 = 1;
13522 } else {
13523 b_kstr++;
13524 }
13525 } else {
13526 b_bool = true;
13527 exitg1 = 1;
13528 }
13529 } while (exitg1 == 0);
13530 }
13531
13532 if (b_bool) {
13533 b_kstr = 0;
13534 } else {
13535 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13536 b_0[b_kstr] = tmp_6[b_kstr];
13537 }
13538
13539 b_bool = false;
13540 if (switch_expression->size[1] == 9) {
13541 b_kstr = 1;
13542 do {
13543 exitg1 = 0;
13544 if (b_kstr - 1 < 9) {
13545 loop_ub = b_kstr - 1;
13546 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
13547 exitg1 = 1;
13548 } else {
13549 b_kstr++;
13550 }
13551 } else {
13552 b_bool = true;
13553 exitg1 = 1;
13554 }
13555 } while (exitg1 == 0);
13556 }
13557
13558 if (b_bool) {
13559 b_kstr = 1;
13560 } else {
13561 b_kstr = -1;
13562 }
13563 }
13564
13565 cartesian_waypoi_emxFree_char_T(&switch_expression);
13566 switch (b_kstr) {
13567 case 0:
13568 tmp[0] = 0;
13569 tmp[1] = 0;
13570 tmp[2] = 1;
13571 tmp[3] = 0;
13572 tmp[4] = 0;
13573 tmp[5] = 0;
13574 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13575 msubspace_data[b_kstr] = tmp[b_kstr];
13576 }
13577
13578 poslim_data[0] = -3.1415926535897931;
13579 poslim_data[1] = 3.1415926535897931;
13580 obj->Base.JointInternal.VelocityNumber = 1.0;
13581 obj->Base.JointInternal.PositionNumber = 1.0;
13582 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
13583 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
13584 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
13585 break;
13586
13587 case 1:
13588 tmp[0] = 0;
13589 tmp[1] = 0;
13590 tmp[2] = 0;
13591 tmp[3] = 0;
13592 tmp[4] = 0;
13593 tmp[5] = 1;
13594 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13595 msubspace_data[b_kstr] = tmp[b_kstr];
13596 }
13597
13598 poslim_data[0] = -0.5;
13599 poslim_data[1] = 0.5;
13600 obj->Base.JointInternal.VelocityNumber = 1.0;
13601 obj->Base.JointInternal.PositionNumber = 1.0;
13602 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
13603 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
13604 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
13605 break;
13606
13607 default:
13608 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13609 msubspace_data[b_kstr] = 0;
13610 }
13611
13612 poslim_data[0] = 0.0;
13613 poslim_data[1] = 0.0;
13614 obj->Base.JointInternal.VelocityNumber = 0.0;
13615 obj->Base.JointInternal.PositionNumber = 0.0;
13616 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
13617 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
13618 obj->Base.JointInternal.JointAxisInternal[2] = 0.0;
13619 break;
13620 }
13621
13622 b_kstr = obj->Base.JointInternal.MotionSubspace->size[0] *
13623 obj->Base.JointInternal.MotionSubspace->size[1];
13624 obj->Base.JointInternal.MotionSubspace->size[0] = 6;
13625 obj->Base.JointInternal.MotionSubspace->size[1] = 1;
13626 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.MotionSubspace, b_kstr);
13627 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13628 obj_0->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
13629 }
13630
13631 b_kstr = obj->Base.JointInternal.PositionLimitsInternal->size[0] *
13632 obj->Base.JointInternal.PositionLimitsInternal->size[1];
13633 obj->Base.JointInternal.PositionLimitsInternal->size[0] = 1;
13634 obj->Base.JointInternal.PositionLimitsInternal->size[1] = 2;
13635 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.PositionLimitsInternal,
13636 b_kstr);
13637 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
13638 obj_0->JointInternal.PositionLimitsInternal->data[b_kstr] =
13639 poslim_data[b_kstr];
13640 }
13641
13642 b_kstr = obj->Base.JointInternal.HomePositionInternal->size[0];
13643 obj->Base.JointInternal.HomePositionInternal->size[0] = 1;
13644 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.HomePositionInternal,
13645 b_kstr);
13646 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
13647 obj_0->JointInternal.HomePositionInternal->data[0] = 0.0;
13648 }
13649
13650 obj->Base.Index = -1.0;
13651 obj->Base.ParentIndex = -1.0;
13652 obj->Base.MassInternal = 1.0;
13653 obj->Base.CenterOfMassInternal[0] = 0.0;
13654 obj->Base.CenterOfMassInternal[1] = 0.0;
13655 obj->Base.CenterOfMassInternal[2] = 0.0;
13656 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13657 b_I[b_kstr] = 0;
13658 }
13659
13660 b_I[0] = 1;
13661 b_I[4] = 1;
13662 b_I[8] = 1;
13663 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13664 obj->Base.InertiaInternal[b_kstr] = b_I[b_kstr];
13665 }
13666
13667 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
13668 msubspace_data[b_kstr] = 0;
13669 }
13670
13671 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13672 msubspace_data[b_kstr + 6 * b_kstr] = 1;
13673 }
13674
13675 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
13676 obj->Base.SpatialInertia[b_kstr] = msubspace_data[b_kstr];
13677 }
13678
13679 return b_obj;
13680}
13681
13682static void cartesian_waypoints_planne_rand(real_T r[5])
13683{
13684 for (cartesian_waypoints_planner_B.b_k = 0; cartesian_waypoints_planner_B.b_k <
13685 5; cartesian_waypoints_planner_B.b_k++) {
13686 memcpy(&cartesian_waypoints_planner_B.uv[0],
13687 &cartesian_waypoints_planner_DW.state_m[0], 625U * sizeof(uint32_T));
13688 cartesian__eml_rand_mt19937ar_e(cartesian_waypoints_planner_B.uv,
13689 cartesian_waypoints_planner_DW.state_m,
13690 &r[cartesian_waypoints_planner_B.b_k]);
13691 }
13692}
13693
13694static w_robotics_manip_internal_Rig_T *RigidBody_RigidBod_evqusngvo0ar
13695 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0)
13696{
13697 w_robotics_manip_internal_Rig_T *b_obj;
13698 int8_T msubspace_data[36];
13699 real_T poslim_data[12];
13700 emxArray_char_T_cartesian_way_T *switch_expression;
13701 boolean_T b_bool;
13702 int32_T b_kstr;
13703 char_T b[8];
13704 char_T b_0[9];
13705 int32_T loop_ub;
13706 int8_T tmp[6];
13707 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
13708 '\x01' };
13709
13710 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
13711 1 };
13712
13713 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
13714 '\x01', '_', 'j', 'n', 't' };
13715
13716 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
13717
13718 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
13719
13720 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
13721
13722 int32_T exitg1;
13723 b_obj = obj;
13724 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
13725 obj->NameInternal->size[0] = 1;
13726 obj->NameInternal->size[1] = 10;
13727 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
13728 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
13729 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
13730 }
13731
13732 iobj_0->InTree = false;
13733 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13734 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
13735 }
13736
13737 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13738 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
13739 }
13740
13741 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
13742 iobj_0->NameInternal->size[0] = 1;
13743 iobj_0->NameInternal->size[1] = 14;
13744 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
13745 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
13746 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
13747 }
13748
13749 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
13750 iobj_0->Type->size[0] = 1;
13751 iobj_0->Type->size[1] = 5;
13752 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
13753 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
13754 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
13755 }
13756
13757 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
13758 b_kstr = switch_expression->size[0] * switch_expression->size[1];
13759 switch_expression->size[0] = 1;
13760 switch_expression->size[1] = iobj_0->Type->size[1];
13761 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
13762 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
13763 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
13764 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
13765 }
13766
13767 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13768 b[b_kstr] = tmp_4[b_kstr];
13769 }
13770
13771 b_bool = false;
13772 if (switch_expression->size[1] == 8) {
13773 b_kstr = 1;
13774 do {
13775 exitg1 = 0;
13776 if (b_kstr - 1 < 8) {
13777 loop_ub = b_kstr - 1;
13778 if (switch_expression->data[loop_ub] != b[loop_ub]) {
13779 exitg1 = 1;
13780 } else {
13781 b_kstr++;
13782 }
13783 } else {
13784 b_bool = true;
13785 exitg1 = 1;
13786 }
13787 } while (exitg1 == 0);
13788 }
13789
13790 if (b_bool) {
13791 b_kstr = 0;
13792 } else {
13793 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13794 b_0[b_kstr] = tmp_5[b_kstr];
13795 }
13796
13797 b_bool = false;
13798 if (switch_expression->size[1] == 9) {
13799 b_kstr = 1;
13800 do {
13801 exitg1 = 0;
13802 if (b_kstr - 1 < 9) {
13803 loop_ub = b_kstr - 1;
13804 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
13805 exitg1 = 1;
13806 } else {
13807 b_kstr++;
13808 }
13809 } else {
13810 b_bool = true;
13811 exitg1 = 1;
13812 }
13813 } while (exitg1 == 0);
13814 }
13815
13816 if (b_bool) {
13817 b_kstr = 1;
13818 } else {
13819 b_kstr = -1;
13820 }
13821 }
13822
13823 cartesian_waypoi_emxFree_char_T(&switch_expression);
13824 switch (b_kstr) {
13825 case 0:
13826 tmp[0] = 0;
13827 tmp[1] = 0;
13828 tmp[2] = 1;
13829 tmp[3] = 0;
13830 tmp[4] = 0;
13831 tmp[5] = 0;
13832 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13833 msubspace_data[b_kstr] = tmp[b_kstr];
13834 }
13835
13836 poslim_data[0] = -3.1415926535897931;
13837 poslim_data[1] = 3.1415926535897931;
13838 iobj_0->VelocityNumber = 1.0;
13839 iobj_0->PositionNumber = 1.0;
13840 iobj_0->JointAxisInternal[0] = 0.0;
13841 iobj_0->JointAxisInternal[1] = 0.0;
13842 iobj_0->JointAxisInternal[2] = 1.0;
13843 break;
13844
13845 case 1:
13846 tmp[0] = 0;
13847 tmp[1] = 0;
13848 tmp[2] = 0;
13849 tmp[3] = 0;
13850 tmp[4] = 0;
13851 tmp[5] = 1;
13852 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13853 msubspace_data[b_kstr] = tmp[b_kstr];
13854 }
13855
13856 poslim_data[0] = -0.5;
13857 poslim_data[1] = 0.5;
13858 iobj_0->VelocityNumber = 1.0;
13859 iobj_0->PositionNumber = 1.0;
13860 iobj_0->JointAxisInternal[0] = 0.0;
13861 iobj_0->JointAxisInternal[1] = 0.0;
13862 iobj_0->JointAxisInternal[2] = 1.0;
13863 break;
13864
13865 default:
13866 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13867 msubspace_data[b_kstr] = 0;
13868 }
13869
13870 poslim_data[0] = 0.0;
13871 poslim_data[1] = 0.0;
13872 iobj_0->VelocityNumber = 0.0;
13873 iobj_0->PositionNumber = 0.0;
13874 iobj_0->JointAxisInternal[0] = 0.0;
13875 iobj_0->JointAxisInternal[1] = 0.0;
13876 iobj_0->JointAxisInternal[2] = 0.0;
13877 break;
13878 }
13879
13880 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
13881 iobj_0->MotionSubspace->size[0] = 6;
13882 iobj_0->MotionSubspace->size[1] = 1;
13883 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
13884 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13885 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
13886 }
13887
13888 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
13889 iobj_0->PositionLimitsInternal->size[1];
13890 iobj_0->PositionLimitsInternal->size[0] = 1;
13891 iobj_0->PositionLimitsInternal->size[1] = 2;
13892 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
13893 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
13894 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
13895 }
13896
13897 b_kstr = iobj_0->HomePositionInternal->size[0];
13898 iobj_0->HomePositionInternal->size[0] = 1;
13899 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
13900 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
13901 iobj_0->HomePositionInternal->data[0] = 0.0;
13902 }
13903
13904 obj->JointInternal = iobj_0;
13905 obj->Index = -1.0;
13906 obj->ParentIndex = -1.0;
13907 return b_obj;
13908}
13909
13910static w_robotics_manip_internal_Rig_T *RigidBody_RigidBo_evqusngvo0ar1
13911 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0)
13912{
13913 w_robotics_manip_internal_Rig_T *b_obj;
13914 int8_T msubspace_data[36];
13915 real_T poslim_data[12];
13916 emxArray_char_T_cartesian_way_T *switch_expression;
13917 boolean_T b_bool;
13918 int32_T b_kstr;
13919 char_T b[8];
13920 char_T b_0[9];
13921 int32_T loop_ub;
13922 int8_T tmp[6];
13923 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
13924 '\x02' };
13925
13926 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
13927 1 };
13928
13929 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
13930 '\x02', '_', 'j', 'n', 't' };
13931
13932 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
13933
13934 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
13935
13936 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
13937
13938 int32_T exitg1;
13939 b_obj = obj;
13940 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
13941 obj->NameInternal->size[0] = 1;
13942 obj->NameInternal->size[1] = 10;
13943 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
13944 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
13945 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
13946 }
13947
13948 iobj_0->InTree = false;
13949 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13950 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
13951 }
13952
13953 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13954 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
13955 }
13956
13957 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
13958 iobj_0->NameInternal->size[0] = 1;
13959 iobj_0->NameInternal->size[1] = 14;
13960 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
13961 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
13962 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
13963 }
13964
13965 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
13966 iobj_0->Type->size[0] = 1;
13967 iobj_0->Type->size[1] = 5;
13968 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
13969 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
13970 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
13971 }
13972
13973 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
13974 b_kstr = switch_expression->size[0] * switch_expression->size[1];
13975 switch_expression->size[0] = 1;
13976 switch_expression->size[1] = iobj_0->Type->size[1];
13977 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
13978 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
13979 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
13980 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
13981 }
13982
13983 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13984 b[b_kstr] = tmp_4[b_kstr];
13985 }
13986
13987 b_bool = false;
13988 if (switch_expression->size[1] == 8) {
13989 b_kstr = 1;
13990 do {
13991 exitg1 = 0;
13992 if (b_kstr - 1 < 8) {
13993 loop_ub = b_kstr - 1;
13994 if (switch_expression->data[loop_ub] != b[loop_ub]) {
13995 exitg1 = 1;
13996 } else {
13997 b_kstr++;
13998 }
13999 } else {
14000 b_bool = true;
14001 exitg1 = 1;
14002 }
14003 } while (exitg1 == 0);
14004 }
14005
14006 if (b_bool) {
14007 b_kstr = 0;
14008 } else {
14009 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14010 b_0[b_kstr] = tmp_5[b_kstr];
14011 }
14012
14013 b_bool = false;
14014 if (switch_expression->size[1] == 9) {
14015 b_kstr = 1;
14016 do {
14017 exitg1 = 0;
14018 if (b_kstr - 1 < 9) {
14019 loop_ub = b_kstr - 1;
14020 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14021 exitg1 = 1;
14022 } else {
14023 b_kstr++;
14024 }
14025 } else {
14026 b_bool = true;
14027 exitg1 = 1;
14028 }
14029 } while (exitg1 == 0);
14030 }
14031
14032 if (b_bool) {
14033 b_kstr = 1;
14034 } else {
14035 b_kstr = -1;
14036 }
14037 }
14038
14039 cartesian_waypoi_emxFree_char_T(&switch_expression);
14040 switch (b_kstr) {
14041 case 0:
14042 tmp[0] = 0;
14043 tmp[1] = 0;
14044 tmp[2] = 1;
14045 tmp[3] = 0;
14046 tmp[4] = 0;
14047 tmp[5] = 0;
14048 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14049 msubspace_data[b_kstr] = tmp[b_kstr];
14050 }
14051
14052 poslim_data[0] = -3.1415926535897931;
14053 poslim_data[1] = 3.1415926535897931;
14054 iobj_0->VelocityNumber = 1.0;
14055 iobj_0->PositionNumber = 1.0;
14056 iobj_0->JointAxisInternal[0] = 0.0;
14057 iobj_0->JointAxisInternal[1] = 0.0;
14058 iobj_0->JointAxisInternal[2] = 1.0;
14059 break;
14060
14061 case 1:
14062 tmp[0] = 0;
14063 tmp[1] = 0;
14064 tmp[2] = 0;
14065 tmp[3] = 0;
14066 tmp[4] = 0;
14067 tmp[5] = 1;
14068 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14069 msubspace_data[b_kstr] = tmp[b_kstr];
14070 }
14071
14072 poslim_data[0] = -0.5;
14073 poslim_data[1] = 0.5;
14074 iobj_0->VelocityNumber = 1.0;
14075 iobj_0->PositionNumber = 1.0;
14076 iobj_0->JointAxisInternal[0] = 0.0;
14077 iobj_0->JointAxisInternal[1] = 0.0;
14078 iobj_0->JointAxisInternal[2] = 1.0;
14079 break;
14080
14081 default:
14082 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14083 msubspace_data[b_kstr] = 0;
14084 }
14085
14086 poslim_data[0] = 0.0;
14087 poslim_data[1] = 0.0;
14088 iobj_0->VelocityNumber = 0.0;
14089 iobj_0->PositionNumber = 0.0;
14090 iobj_0->JointAxisInternal[0] = 0.0;
14091 iobj_0->JointAxisInternal[1] = 0.0;
14092 iobj_0->JointAxisInternal[2] = 0.0;
14093 break;
14094 }
14095
14096 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
14097 iobj_0->MotionSubspace->size[0] = 6;
14098 iobj_0->MotionSubspace->size[1] = 1;
14099 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
14100 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14101 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14102 }
14103
14104 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
14105 iobj_0->PositionLimitsInternal->size[1];
14106 iobj_0->PositionLimitsInternal->size[0] = 1;
14107 iobj_0->PositionLimitsInternal->size[1] = 2;
14108 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
14109 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14110 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14111 }
14112
14113 b_kstr = iobj_0->HomePositionInternal->size[0];
14114 iobj_0->HomePositionInternal->size[0] = 1;
14115 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
14116 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14117 iobj_0->HomePositionInternal->data[0] = 0.0;
14118 }
14119
14120 obj->JointInternal = iobj_0;
14121 obj->Index = -1.0;
14122 obj->ParentIndex = -1.0;
14123 return b_obj;
14124}
14125
14126static w_robotics_manip_internal_Rig_T *RigidBody_RigidB_evqusngvo0ar1d
14127 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0)
14128{
14129 w_robotics_manip_internal_Rig_T *b_obj;
14130 int8_T msubspace_data[36];
14131 real_T poslim_data[12];
14132 emxArray_char_T_cartesian_way_T *switch_expression;
14133 boolean_T b_bool;
14134 int32_T b_kstr;
14135 char_T b[8];
14136 char_T b_0[9];
14137 int32_T loop_ub;
14138 int8_T tmp[6];
14139 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14140 '\x03' };
14141
14142 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14143 1 };
14144
14145 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14146 '\x03', '_', 'j', 'n', 't' };
14147
14148 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
14149
14150 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14151
14152 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14153
14154 int32_T exitg1;
14155 b_obj = obj;
14156 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
14157 obj->NameInternal->size[0] = 1;
14158 obj->NameInternal->size[1] = 10;
14159 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
14160 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
14161 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14162 }
14163
14164 iobj_0->InTree = false;
14165 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14166 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
14167 }
14168
14169 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14170 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
14171 }
14172
14173 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
14174 iobj_0->NameInternal->size[0] = 1;
14175 iobj_0->NameInternal->size[1] = 14;
14176 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
14177 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
14178 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
14179 }
14180
14181 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
14182 iobj_0->Type->size[0] = 1;
14183 iobj_0->Type->size[1] = 5;
14184 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
14185 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
14186 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
14187 }
14188
14189 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
14190 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14191 switch_expression->size[0] = 1;
14192 switch_expression->size[1] = iobj_0->Type->size[1];
14193 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
14194 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
14195 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14196 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
14197 }
14198
14199 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14200 b[b_kstr] = tmp_4[b_kstr];
14201 }
14202
14203 b_bool = false;
14204 if (switch_expression->size[1] == 8) {
14205 b_kstr = 1;
14206 do {
14207 exitg1 = 0;
14208 if (b_kstr - 1 < 8) {
14209 loop_ub = b_kstr - 1;
14210 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14211 exitg1 = 1;
14212 } else {
14213 b_kstr++;
14214 }
14215 } else {
14216 b_bool = true;
14217 exitg1 = 1;
14218 }
14219 } while (exitg1 == 0);
14220 }
14221
14222 if (b_bool) {
14223 b_kstr = 0;
14224 } else {
14225 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14226 b_0[b_kstr] = tmp_5[b_kstr];
14227 }
14228
14229 b_bool = false;
14230 if (switch_expression->size[1] == 9) {
14231 b_kstr = 1;
14232 do {
14233 exitg1 = 0;
14234 if (b_kstr - 1 < 9) {
14235 loop_ub = b_kstr - 1;
14236 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14237 exitg1 = 1;
14238 } else {
14239 b_kstr++;
14240 }
14241 } else {
14242 b_bool = true;
14243 exitg1 = 1;
14244 }
14245 } while (exitg1 == 0);
14246 }
14247
14248 if (b_bool) {
14249 b_kstr = 1;
14250 } else {
14251 b_kstr = -1;
14252 }
14253 }
14254
14255 cartesian_waypoi_emxFree_char_T(&switch_expression);
14256 switch (b_kstr) {
14257 case 0:
14258 tmp[0] = 0;
14259 tmp[1] = 0;
14260 tmp[2] = 1;
14261 tmp[3] = 0;
14262 tmp[4] = 0;
14263 tmp[5] = 0;
14264 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14265 msubspace_data[b_kstr] = tmp[b_kstr];
14266 }
14267
14268 poslim_data[0] = -3.1415926535897931;
14269 poslim_data[1] = 3.1415926535897931;
14270 iobj_0->VelocityNumber = 1.0;
14271 iobj_0->PositionNumber = 1.0;
14272 iobj_0->JointAxisInternal[0] = 0.0;
14273 iobj_0->JointAxisInternal[1] = 0.0;
14274 iobj_0->JointAxisInternal[2] = 1.0;
14275 break;
14276
14277 case 1:
14278 tmp[0] = 0;
14279 tmp[1] = 0;
14280 tmp[2] = 0;
14281 tmp[3] = 0;
14282 tmp[4] = 0;
14283 tmp[5] = 1;
14284 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14285 msubspace_data[b_kstr] = tmp[b_kstr];
14286 }
14287
14288 poslim_data[0] = -0.5;
14289 poslim_data[1] = 0.5;
14290 iobj_0->VelocityNumber = 1.0;
14291 iobj_0->PositionNumber = 1.0;
14292 iobj_0->JointAxisInternal[0] = 0.0;
14293 iobj_0->JointAxisInternal[1] = 0.0;
14294 iobj_0->JointAxisInternal[2] = 1.0;
14295 break;
14296
14297 default:
14298 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14299 msubspace_data[b_kstr] = 0;
14300 }
14301
14302 poslim_data[0] = 0.0;
14303 poslim_data[1] = 0.0;
14304 iobj_0->VelocityNumber = 0.0;
14305 iobj_0->PositionNumber = 0.0;
14306 iobj_0->JointAxisInternal[0] = 0.0;
14307 iobj_0->JointAxisInternal[1] = 0.0;
14308 iobj_0->JointAxisInternal[2] = 0.0;
14309 break;
14310 }
14311
14312 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
14313 iobj_0->MotionSubspace->size[0] = 6;
14314 iobj_0->MotionSubspace->size[1] = 1;
14315 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
14316 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14317 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14318 }
14319
14320 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
14321 iobj_0->PositionLimitsInternal->size[1];
14322 iobj_0->PositionLimitsInternal->size[0] = 1;
14323 iobj_0->PositionLimitsInternal->size[1] = 2;
14324 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
14325 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14326 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14327 }
14328
14329 b_kstr = iobj_0->HomePositionInternal->size[0];
14330 iobj_0->HomePositionInternal->size[0] = 1;
14331 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
14332 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14333 iobj_0->HomePositionInternal->data[0] = 0.0;
14334 }
14335
14336 obj->JointInternal = iobj_0;
14337 obj->Index = -1.0;
14338 obj->ParentIndex = -1.0;
14339 return b_obj;
14340}
14341
14342static w_robotics_manip_internal_Rig_T *RigidBody_Rigid_evqusngvo0ar1db
14343 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0)
14344{
14345 w_robotics_manip_internal_Rig_T *b_obj;
14346 int8_T msubspace_data[36];
14347 real_T poslim_data[12];
14348 emxArray_char_T_cartesian_way_T *switch_expression;
14349 boolean_T b_bool;
14350 int32_T b_kstr;
14351 char_T b[8];
14352 char_T b_0[9];
14353 int32_T loop_ub;
14354 int8_T tmp[6];
14355 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14356 '\x04' };
14357
14358 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14359 1 };
14360
14361 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14362 '\x04', '_', 'j', 'n', 't' };
14363
14364 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
14365
14366 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14367
14368 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14369
14370 int32_T exitg1;
14371 b_obj = obj;
14372 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
14373 obj->NameInternal->size[0] = 1;
14374 obj->NameInternal->size[1] = 10;
14375 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
14376 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
14377 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14378 }
14379
14380 iobj_0->InTree = false;
14381 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14382 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
14383 }
14384
14385 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14386 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
14387 }
14388
14389 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
14390 iobj_0->NameInternal->size[0] = 1;
14391 iobj_0->NameInternal->size[1] = 14;
14392 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
14393 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
14394 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
14395 }
14396
14397 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
14398 iobj_0->Type->size[0] = 1;
14399 iobj_0->Type->size[1] = 5;
14400 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
14401 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
14402 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
14403 }
14404
14405 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
14406 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14407 switch_expression->size[0] = 1;
14408 switch_expression->size[1] = iobj_0->Type->size[1];
14409 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
14410 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
14411 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14412 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
14413 }
14414
14415 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14416 b[b_kstr] = tmp_4[b_kstr];
14417 }
14418
14419 b_bool = false;
14420 if (switch_expression->size[1] == 8) {
14421 b_kstr = 1;
14422 do {
14423 exitg1 = 0;
14424 if (b_kstr - 1 < 8) {
14425 loop_ub = b_kstr - 1;
14426 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14427 exitg1 = 1;
14428 } else {
14429 b_kstr++;
14430 }
14431 } else {
14432 b_bool = true;
14433 exitg1 = 1;
14434 }
14435 } while (exitg1 == 0);
14436 }
14437
14438 if (b_bool) {
14439 b_kstr = 0;
14440 } else {
14441 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14442 b_0[b_kstr] = tmp_5[b_kstr];
14443 }
14444
14445 b_bool = false;
14446 if (switch_expression->size[1] == 9) {
14447 b_kstr = 1;
14448 do {
14449 exitg1 = 0;
14450 if (b_kstr - 1 < 9) {
14451 loop_ub = b_kstr - 1;
14452 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14453 exitg1 = 1;
14454 } else {
14455 b_kstr++;
14456 }
14457 } else {
14458 b_bool = true;
14459 exitg1 = 1;
14460 }
14461 } while (exitg1 == 0);
14462 }
14463
14464 if (b_bool) {
14465 b_kstr = 1;
14466 } else {
14467 b_kstr = -1;
14468 }
14469 }
14470
14471 cartesian_waypoi_emxFree_char_T(&switch_expression);
14472 switch (b_kstr) {
14473 case 0:
14474 tmp[0] = 0;
14475 tmp[1] = 0;
14476 tmp[2] = 1;
14477 tmp[3] = 0;
14478 tmp[4] = 0;
14479 tmp[5] = 0;
14480 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14481 msubspace_data[b_kstr] = tmp[b_kstr];
14482 }
14483
14484 poslim_data[0] = -3.1415926535897931;
14485 poslim_data[1] = 3.1415926535897931;
14486 iobj_0->VelocityNumber = 1.0;
14487 iobj_0->PositionNumber = 1.0;
14488 iobj_0->JointAxisInternal[0] = 0.0;
14489 iobj_0->JointAxisInternal[1] = 0.0;
14490 iobj_0->JointAxisInternal[2] = 1.0;
14491 break;
14492
14493 case 1:
14494 tmp[0] = 0;
14495 tmp[1] = 0;
14496 tmp[2] = 0;
14497 tmp[3] = 0;
14498 tmp[4] = 0;
14499 tmp[5] = 1;
14500 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14501 msubspace_data[b_kstr] = tmp[b_kstr];
14502 }
14503
14504 poslim_data[0] = -0.5;
14505 poslim_data[1] = 0.5;
14506 iobj_0->VelocityNumber = 1.0;
14507 iobj_0->PositionNumber = 1.0;
14508 iobj_0->JointAxisInternal[0] = 0.0;
14509 iobj_0->JointAxisInternal[1] = 0.0;
14510 iobj_0->JointAxisInternal[2] = 1.0;
14511 break;
14512
14513 default:
14514 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14515 msubspace_data[b_kstr] = 0;
14516 }
14517
14518 poslim_data[0] = 0.0;
14519 poslim_data[1] = 0.0;
14520 iobj_0->VelocityNumber = 0.0;
14521 iobj_0->PositionNumber = 0.0;
14522 iobj_0->JointAxisInternal[0] = 0.0;
14523 iobj_0->JointAxisInternal[1] = 0.0;
14524 iobj_0->JointAxisInternal[2] = 0.0;
14525 break;
14526 }
14527
14528 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
14529 iobj_0->MotionSubspace->size[0] = 6;
14530 iobj_0->MotionSubspace->size[1] = 1;
14531 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
14532 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14533 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14534 }
14535
14536 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
14537 iobj_0->PositionLimitsInternal->size[1];
14538 iobj_0->PositionLimitsInternal->size[0] = 1;
14539 iobj_0->PositionLimitsInternal->size[1] = 2;
14540 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
14541 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14542 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14543 }
14544
14545 b_kstr = iobj_0->HomePositionInternal->size[0];
14546 iobj_0->HomePositionInternal->size[0] = 1;
14547 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
14548 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14549 iobj_0->HomePositionInternal->data[0] = 0.0;
14550 }
14551
14552 obj->JointInternal = iobj_0;
14553 obj->Index = -1.0;
14554 obj->ParentIndex = -1.0;
14555 return b_obj;
14556}
14557
14558static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_c
14559 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0)
14560{
14561 w_robotics_manip_internal_Rig_T *b_obj;
14562 int8_T msubspace_data[36];
14563 real_T poslim_data[12];
14564 emxArray_char_T_cartesian_way_T *switch_expression;
14565 boolean_T b_bool;
14566 int32_T b_kstr;
14567 char_T b[8];
14568 char_T b_0[9];
14569 int32_T loop_ub;
14570 int8_T tmp[6];
14571 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14572 '\x05' };
14573
14574 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14575 1 };
14576
14577 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14578 '\x05', '_', 'j', 'n', 't' };
14579
14580 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
14581
14582 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14583
14584 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14585
14586 int32_T exitg1;
14587 b_obj = obj;
14588 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
14589 obj->NameInternal->size[0] = 1;
14590 obj->NameInternal->size[1] = 10;
14591 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
14592 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
14593 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14594 }
14595
14596 iobj_0->InTree = false;
14597 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14598 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
14599 }
14600
14601 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14602 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
14603 }
14604
14605 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
14606 iobj_0->NameInternal->size[0] = 1;
14607 iobj_0->NameInternal->size[1] = 14;
14608 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
14609 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
14610 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
14611 }
14612
14613 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
14614 iobj_0->Type->size[0] = 1;
14615 iobj_0->Type->size[1] = 5;
14616 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
14617 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
14618 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
14619 }
14620
14621 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
14622 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14623 switch_expression->size[0] = 1;
14624 switch_expression->size[1] = iobj_0->Type->size[1];
14625 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
14626 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
14627 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14628 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
14629 }
14630
14631 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14632 b[b_kstr] = tmp_4[b_kstr];
14633 }
14634
14635 b_bool = false;
14636 if (switch_expression->size[1] == 8) {
14637 b_kstr = 1;
14638 do {
14639 exitg1 = 0;
14640 if (b_kstr - 1 < 8) {
14641 loop_ub = b_kstr - 1;
14642 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14643 exitg1 = 1;
14644 } else {
14645 b_kstr++;
14646 }
14647 } else {
14648 b_bool = true;
14649 exitg1 = 1;
14650 }
14651 } while (exitg1 == 0);
14652 }
14653
14654 if (b_bool) {
14655 b_kstr = 0;
14656 } else {
14657 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14658 b_0[b_kstr] = tmp_5[b_kstr];
14659 }
14660
14661 b_bool = false;
14662 if (switch_expression->size[1] == 9) {
14663 b_kstr = 1;
14664 do {
14665 exitg1 = 0;
14666 if (b_kstr - 1 < 9) {
14667 loop_ub = b_kstr - 1;
14668 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14669 exitg1 = 1;
14670 } else {
14671 b_kstr++;
14672 }
14673 } else {
14674 b_bool = true;
14675 exitg1 = 1;
14676 }
14677 } while (exitg1 == 0);
14678 }
14679
14680 if (b_bool) {
14681 b_kstr = 1;
14682 } else {
14683 b_kstr = -1;
14684 }
14685 }
14686
14687 cartesian_waypoi_emxFree_char_T(&switch_expression);
14688 switch (b_kstr) {
14689 case 0:
14690 tmp[0] = 0;
14691 tmp[1] = 0;
14692 tmp[2] = 1;
14693 tmp[3] = 0;
14694 tmp[4] = 0;
14695 tmp[5] = 0;
14696 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14697 msubspace_data[b_kstr] = tmp[b_kstr];
14698 }
14699
14700 poslim_data[0] = -3.1415926535897931;
14701 poslim_data[1] = 3.1415926535897931;
14702 iobj_0->VelocityNumber = 1.0;
14703 iobj_0->PositionNumber = 1.0;
14704 iobj_0->JointAxisInternal[0] = 0.0;
14705 iobj_0->JointAxisInternal[1] = 0.0;
14706 iobj_0->JointAxisInternal[2] = 1.0;
14707 break;
14708
14709 case 1:
14710 tmp[0] = 0;
14711 tmp[1] = 0;
14712 tmp[2] = 0;
14713 tmp[3] = 0;
14714 tmp[4] = 0;
14715 tmp[5] = 1;
14716 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14717 msubspace_data[b_kstr] = tmp[b_kstr];
14718 }
14719
14720 poslim_data[0] = -0.5;
14721 poslim_data[1] = 0.5;
14722 iobj_0->VelocityNumber = 1.0;
14723 iobj_0->PositionNumber = 1.0;
14724 iobj_0->JointAxisInternal[0] = 0.0;
14725 iobj_0->JointAxisInternal[1] = 0.0;
14726 iobj_0->JointAxisInternal[2] = 1.0;
14727 break;
14728
14729 default:
14730 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14731 msubspace_data[b_kstr] = 0;
14732 }
14733
14734 poslim_data[0] = 0.0;
14735 poslim_data[1] = 0.0;
14736 iobj_0->VelocityNumber = 0.0;
14737 iobj_0->PositionNumber = 0.0;
14738 iobj_0->JointAxisInternal[0] = 0.0;
14739 iobj_0->JointAxisInternal[1] = 0.0;
14740 iobj_0->JointAxisInternal[2] = 0.0;
14741 break;
14742 }
14743
14744 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
14745 iobj_0->MotionSubspace->size[0] = 6;
14746 iobj_0->MotionSubspace->size[1] = 1;
14747 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
14748 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14749 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14750 }
14751
14752 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
14753 iobj_0->PositionLimitsInternal->size[1];
14754 iobj_0->PositionLimitsInternal->size[0] = 1;
14755 iobj_0->PositionLimitsInternal->size[1] = 2;
14756 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
14757 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14758 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14759 }
14760
14761 b_kstr = iobj_0->HomePositionInternal->size[0];
14762 iobj_0->HomePositionInternal->size[0] = 1;
14763 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
14764 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14765 iobj_0->HomePositionInternal->data[0] = 0.0;
14766 }
14767
14768 obj->JointInternal = iobj_0;
14769 obj->Index = -1.0;
14770 obj->ParentIndex = -1.0;
14771 return b_obj;
14772}
14773
14774static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
14775 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
14776 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
14777 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
14778 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
14779 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian__e_T
14780 *iobj_7, c_rigidBodyJoint_cartesian__e_T *iobj_8,
14781 c_rigidBodyJoint_cartesian__e_T *iobj_9, c_rigidBodyJoint_cartesian__e_T
14782 *iobj_10, c_rigidBodyJoint_cartesian__e_T *iobj_11,
14783 c_rigidBodyJoint_cartesian__e_T *iobj_12, c_rigidBodyJoint_cartesian__e_T
14784 *iobj_13, c_rigidBodyJoint_cartesian__e_T *iobj_14,
14785 w_robotics_manip_internal_Rig_T *iobj_15)
14786{
14787 emxArray_char_T_cartesian_way_T *switch_expression;
14788 static const char_T tmp[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14789 '\x06' };
14790
14791 static const int8_T tmp_0[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14792 1 };
14793
14794 static const char_T tmp_1[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14795 '\x06', '_', 'j', 'n', 't' };
14796
14797 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
14798
14799 static const char_T tmp_3[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14800
14801 static const char_T tmp_4[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14802
14803 static const char_T tmp_5[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14804 '\x07' };
14805
14806 static const char_T tmp_6[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14807 '\x07', '_', 'j', 'n', 't' };
14808
14809 static const char_T tmp_7[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14810 '\x08' };
14811
14812 static const char_T tmp_8[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
14813 '\x08', '_', 'j', 'n', 't' };
14814
14815 int32_T exitg1;
14816 obj->Bodies[0] = RigidBody_RigidBod_evqusngvo0ar(iobj_15, iobj_14);
14817 obj->Bodies[1] = RigidBody_RigidBo_evqusngvo0ar1(iobj_0, iobj_7);
14818 obj->Bodies[2] = RigidBody_RigidB_evqusngvo0ar1d(iobj_1, iobj_8);
14819 obj->Bodies[3] = RigidBody_Rigid_evqusngvo0ar1db(iobj_2, iobj_9);
14820 obj->Bodies[4] = c_RigidBody_Rigid_c(iobj_3, iobj_10);
14821 cartesian_waypoints_planner_B.b_kstr_m = iobj_4->NameInternal->size[0] *
14822 iobj_4->NameInternal->size[1];
14823 iobj_4->NameInternal->size[0] = 1;
14824 iobj_4->NameInternal->size[1] = 10;
14825 cartes_emxEnsureCapacity_char_T(iobj_4->NameInternal,
14826 cartesian_waypoints_planner_B.b_kstr_m);
14827 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
14828 cartesian_waypoints_planner_B.b_kstr_m < 10;
14829 cartesian_waypoints_planner_B.b_kstr_m++) {
14830 iobj_4->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_m] =
14831 tmp[cartesian_waypoints_planner_B.b_kstr_m];
14832 }
14833
14834 iobj_11->InTree = false;
14835 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
14836 cartesian_waypoints_planner_B.b_kstr_m < 16;
14837 cartesian_waypoints_planner_B.b_kstr_m++) {
14838 iobj_11->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_m] =
14839 tmp_0[cartesian_waypoints_planner_B.b_kstr_m];
14840 }
14841
14842 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
14843 cartesian_waypoints_planner_B.b_kstr_m < 16;
14844 cartesian_waypoints_planner_B.b_kstr_m++) {
14845 iobj_11->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_m] =
14846 tmp_0[cartesian_waypoints_planner_B.b_kstr_m];
14847 }
14848
14849 cartesian_waypoints_planner_B.b_kstr_m = iobj_11->NameInternal->size[0] *
14850 iobj_11->NameInternal->size[1];
14851 iobj_11->NameInternal->size[0] = 1;
14852 iobj_11->NameInternal->size[1] = 14;
14853 cartes_emxEnsureCapacity_char_T(iobj_11->NameInternal,
14854 cartesian_waypoints_planner_B.b_kstr_m);
14855 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
14856 cartesian_waypoints_planner_B.b_kstr_m < 14;
14857 cartesian_waypoints_planner_B.b_kstr_m++) {
14858 iobj_11->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_m] =
14859 tmp_1[cartesian_waypoints_planner_B.b_kstr_m];
14860 }
14861
14862 cartesian_waypoints_planner_B.b_kstr_m = iobj_11->Type->size[0] *
14863 iobj_11->Type->size[1];
14864 iobj_11->Type->size[0] = 1;
14865 iobj_11->Type->size[1] = 5;
14866 cartes_emxEnsureCapacity_char_T(iobj_11->Type,
14867 cartesian_waypoints_planner_B.b_kstr_m);
14868 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
14869 cartesian_waypoints_planner_B.b_kstr_m < 5;
14870 cartesian_waypoints_planner_B.b_kstr_m++) {
14871 iobj_11->Type->data[cartesian_waypoints_planner_B.b_kstr_m] =
14872 tmp_2[cartesian_waypoints_planner_B.b_kstr_m];
14873 }
14874
14875 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
14876 cartesian_waypoints_planner_B.b_kstr_m = switch_expression->size[0] *
14877 switch_expression->size[1];
14878 switch_expression->size[0] = 1;
14879 switch_expression->size[1] = iobj_11->Type->size[1];
14880 cartes_emxEnsureCapacity_char_T(switch_expression,
14881 cartesian_waypoints_planner_B.b_kstr_m);
14882 cartesian_waypoints_planner_B.loop_ub_n = iobj_11->Type->size[0] *
14883 iobj_11->Type->size[1] - 1;
14884 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
14885 cartesian_waypoints_planner_B.b_kstr_m <=
14886 cartesian_waypoints_planner_B.loop_ub_n;
14887 cartesian_waypoints_planner_B.b_kstr_m++) {
14888 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_m] =
14889 iobj_11->Type->data[cartesian_waypoints_planner_B.b_kstr_m];
14890 }
14891
14892 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
14893 cartesian_waypoints_planner_B.b_kstr_m < 8;
14894 cartesian_waypoints_planner_B.b_kstr_m++) {
14895 cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.b_kstr_m] =
14896 tmp_3[cartesian_waypoints_planner_B.b_kstr_m];
14897 }
14898
14899 cartesian_waypoints_planner_B.b_bool_nh = false;
14900 if (switch_expression->size[1] == 8) {
14901 cartesian_waypoints_planner_B.b_kstr_m = 1;
14902 do {
14903 exitg1 = 0;
14904 if (cartesian_waypoints_planner_B.b_kstr_m - 1 < 8) {
14905 cartesian_waypoints_planner_B.loop_ub_n =
14906 cartesian_waypoints_planner_B.b_kstr_m - 1;
14907 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_n] !=
14908 cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.loop_ub_n])
14909 {
14910 exitg1 = 1;
14911 } else {
14912 cartesian_waypoints_planner_B.b_kstr_m++;
14913 }
14914 } else {
14915 cartesian_waypoints_planner_B.b_bool_nh = true;
14916 exitg1 = 1;
14917 }
14918 } while (exitg1 == 0);
14919 }
14920
14921 if (cartesian_waypoints_planner_B.b_bool_nh) {
14922 cartesian_waypoints_planner_B.b_kstr_m = 0;
14923 } else {
14924 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
14925 cartesian_waypoints_planner_B.b_kstr_m < 9;
14926 cartesian_waypoints_planner_B.b_kstr_m++) {
14927 cartesian_waypoints_planner_B.b_c[cartesian_waypoints_planner_B.b_kstr_m] =
14928 tmp_4[cartesian_waypoints_planner_B.b_kstr_m];
14929 }
14930
14931 cartesian_waypoints_planner_B.b_bool_nh = false;
14932 if (switch_expression->size[1] == 9) {
14933 cartesian_waypoints_planner_B.b_kstr_m = 1;
14934 do {
14935 exitg1 = 0;
14936 if (cartesian_waypoints_planner_B.b_kstr_m - 1 < 9) {
14937 cartesian_waypoints_planner_B.loop_ub_n =
14938 cartesian_waypoints_planner_B.b_kstr_m - 1;
14939 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_n]
14940 !=
14941 cartesian_waypoints_planner_B.b_c[cartesian_waypoints_planner_B.loop_ub_n])
14942 {
14943 exitg1 = 1;
14944 } else {
14945 cartesian_waypoints_planner_B.b_kstr_m++;
14946 }
14947 } else {
14948 cartesian_waypoints_planner_B.b_bool_nh = true;
14949 exitg1 = 1;
14950 }
14951 } while (exitg1 == 0);
14952 }
14953
14954 if (cartesian_waypoints_planner_B.b_bool_nh) {
14955 cartesian_waypoints_planner_B.b_kstr_m = 1;
14956 } else {
14957 cartesian_waypoints_planner_B.b_kstr_m = -1;
14958 }
14959 }
14960
14961 switch (cartesian_waypoints_planner_B.b_kstr_m) {
14962 case 0:
14963 cartesian_waypoints_planner_B.iv1[0] = 0;
14964 cartesian_waypoints_planner_B.iv1[1] = 0;
14965 cartesian_waypoints_planner_B.iv1[2] = 1;
14966 cartesian_waypoints_planner_B.iv1[3] = 0;
14967 cartesian_waypoints_planner_B.iv1[4] = 0;
14968 cartesian_waypoints_planner_B.iv1[5] = 0;
14969 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
14970 cartesian_waypoints_planner_B.b_kstr_m < 6;
14971 cartesian_waypoints_planner_B.b_kstr_m++) {
14972 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m]
14973 =
14974 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_m];
14975 }
14976
14977 cartesian_waypoints_planner_B.poslim_data_p[0] = -3.1415926535897931;
14978 cartesian_waypoints_planner_B.poslim_data_p[1] = 3.1415926535897931;
14979 iobj_11->VelocityNumber = 1.0;
14980 iobj_11->PositionNumber = 1.0;
14981 iobj_11->JointAxisInternal[0] = 0.0;
14982 iobj_11->JointAxisInternal[1] = 0.0;
14983 iobj_11->JointAxisInternal[2] = 1.0;
14984 break;
14985
14986 case 1:
14987 cartesian_waypoints_planner_B.iv1[0] = 0;
14988 cartesian_waypoints_planner_B.iv1[1] = 0;
14989 cartesian_waypoints_planner_B.iv1[2] = 0;
14990 cartesian_waypoints_planner_B.iv1[3] = 0;
14991 cartesian_waypoints_planner_B.iv1[4] = 0;
14992 cartesian_waypoints_planner_B.iv1[5] = 1;
14993 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
14994 cartesian_waypoints_planner_B.b_kstr_m < 6;
14995 cartesian_waypoints_planner_B.b_kstr_m++) {
14996 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m]
14997 =
14998 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_m];
14999 }
15000
15001 cartesian_waypoints_planner_B.poslim_data_p[0] = -0.5;
15002 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.5;
15003 iobj_11->VelocityNumber = 1.0;
15004 iobj_11->PositionNumber = 1.0;
15005 iobj_11->JointAxisInternal[0] = 0.0;
15006 iobj_11->JointAxisInternal[1] = 0.0;
15007 iobj_11->JointAxisInternal[2] = 1.0;
15008 break;
15009
15010 default:
15011 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15012 cartesian_waypoints_planner_B.b_kstr_m < 6;
15013 cartesian_waypoints_planner_B.b_kstr_m++) {
15014 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m]
15015 = 0;
15016 }
15017
15018 cartesian_waypoints_planner_B.poslim_data_p[0] = 0.0;
15019 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.0;
15020 iobj_11->VelocityNumber = 0.0;
15021 iobj_11->PositionNumber = 0.0;
15022 iobj_11->JointAxisInternal[0] = 0.0;
15023 iobj_11->JointAxisInternal[1] = 0.0;
15024 iobj_11->JointAxisInternal[2] = 0.0;
15025 break;
15026 }
15027
15028 cartesian_waypoints_planner_B.b_kstr_m = iobj_11->MotionSubspace->size[0] *
15029 iobj_11->MotionSubspace->size[1];
15030 iobj_11->MotionSubspace->size[0] = 6;
15031 iobj_11->MotionSubspace->size[1] = 1;
15032 cartes_emxEnsureCapacity_real_T(iobj_11->MotionSubspace,
15033 cartesian_waypoints_planner_B.b_kstr_m);
15034 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15035 cartesian_waypoints_planner_B.b_kstr_m < 6;
15036 cartesian_waypoints_planner_B.b_kstr_m++) {
15037 iobj_11->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_m] =
15038 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m];
15039 }
15040
15041 cartesian_waypoints_planner_B.b_kstr_m = iobj_11->PositionLimitsInternal->
15042 size[0] * iobj_11->PositionLimitsInternal->size[1];
15043 iobj_11->PositionLimitsInternal->size[0] = 1;
15044 iobj_11->PositionLimitsInternal->size[1] = 2;
15045 cartes_emxEnsureCapacity_real_T(iobj_11->PositionLimitsInternal,
15046 cartesian_waypoints_planner_B.b_kstr_m);
15047 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15048 cartesian_waypoints_planner_B.b_kstr_m < 2;
15049 cartesian_waypoints_planner_B.b_kstr_m++) {
15050 iobj_11->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_m]
15051 =
15052 cartesian_waypoints_planner_B.poslim_data_p[cartesian_waypoints_planner_B.b_kstr_m];
15053 }
15054
15055 cartesian_waypoints_planner_B.b_kstr_m = iobj_11->HomePositionInternal->size[0];
15056 iobj_11->HomePositionInternal->size[0] = 1;
15057 cartes_emxEnsureCapacity_real_T(iobj_11->HomePositionInternal,
15058 cartesian_waypoints_planner_B.b_kstr_m);
15059 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15060 cartesian_waypoints_planner_B.b_kstr_m < 1;
15061 cartesian_waypoints_planner_B.b_kstr_m++) {
15062 iobj_11->HomePositionInternal->data[0] = 0.0;
15063 }
15064
15065 iobj_4->JointInternal = iobj_11;
15066 iobj_4->Index = -1.0;
15067 iobj_4->ParentIndex = -1.0;
15068 obj->Bodies[5] = iobj_4;
15069 cartesian_waypoints_planner_B.b_kstr_m = iobj_5->NameInternal->size[0] *
15070 iobj_5->NameInternal->size[1];
15071 iobj_5->NameInternal->size[0] = 1;
15072 iobj_5->NameInternal->size[1] = 10;
15073 cartes_emxEnsureCapacity_char_T(iobj_5->NameInternal,
15074 cartesian_waypoints_planner_B.b_kstr_m);
15075 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15076 cartesian_waypoints_planner_B.b_kstr_m < 10;
15077 cartesian_waypoints_planner_B.b_kstr_m++) {
15078 iobj_5->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_m] =
15079 tmp_5[cartesian_waypoints_planner_B.b_kstr_m];
15080 }
15081
15082 iobj_12->InTree = false;
15083 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15084 cartesian_waypoints_planner_B.b_kstr_m < 16;
15085 cartesian_waypoints_planner_B.b_kstr_m++) {
15086 iobj_12->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_m] =
15087 tmp_0[cartesian_waypoints_planner_B.b_kstr_m];
15088 }
15089
15090 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15091 cartesian_waypoints_planner_B.b_kstr_m < 16;
15092 cartesian_waypoints_planner_B.b_kstr_m++) {
15093 iobj_12->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_m] =
15094 tmp_0[cartesian_waypoints_planner_B.b_kstr_m];
15095 }
15096
15097 cartesian_waypoints_planner_B.b_kstr_m = iobj_12->NameInternal->size[0] *
15098 iobj_12->NameInternal->size[1];
15099 iobj_12->NameInternal->size[0] = 1;
15100 iobj_12->NameInternal->size[1] = 14;
15101 cartes_emxEnsureCapacity_char_T(iobj_12->NameInternal,
15102 cartesian_waypoints_planner_B.b_kstr_m);
15103 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15104 cartesian_waypoints_planner_B.b_kstr_m < 14;
15105 cartesian_waypoints_planner_B.b_kstr_m++) {
15106 iobj_12->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_m] =
15107 tmp_6[cartesian_waypoints_planner_B.b_kstr_m];
15108 }
15109
15110 cartesian_waypoints_planner_B.b_kstr_m = iobj_12->Type->size[0] *
15111 iobj_12->Type->size[1];
15112 iobj_12->Type->size[0] = 1;
15113 iobj_12->Type->size[1] = 5;
15114 cartes_emxEnsureCapacity_char_T(iobj_12->Type,
15115 cartesian_waypoints_planner_B.b_kstr_m);
15116 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15117 cartesian_waypoints_planner_B.b_kstr_m < 5;
15118 cartesian_waypoints_planner_B.b_kstr_m++) {
15119 iobj_12->Type->data[cartesian_waypoints_planner_B.b_kstr_m] =
15120 tmp_2[cartesian_waypoints_planner_B.b_kstr_m];
15121 }
15122
15123 cartesian_waypoints_planner_B.b_kstr_m = switch_expression->size[0] *
15124 switch_expression->size[1];
15125 switch_expression->size[0] = 1;
15126 switch_expression->size[1] = iobj_12->Type->size[1];
15127 cartes_emxEnsureCapacity_char_T(switch_expression,
15128 cartesian_waypoints_planner_B.b_kstr_m);
15129 cartesian_waypoints_planner_B.loop_ub_n = iobj_12->Type->size[0] *
15130 iobj_12->Type->size[1] - 1;
15131 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15132 cartesian_waypoints_planner_B.b_kstr_m <=
15133 cartesian_waypoints_planner_B.loop_ub_n;
15134 cartesian_waypoints_planner_B.b_kstr_m++) {
15135 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_m] =
15136 iobj_12->Type->data[cartesian_waypoints_planner_B.b_kstr_m];
15137 }
15138
15139 cartesian_waypoints_planner_B.b_bool_nh = false;
15140 if (switch_expression->size[1] == 8) {
15141 cartesian_waypoints_planner_B.b_kstr_m = 1;
15142 do {
15143 exitg1 = 0;
15144 if (cartesian_waypoints_planner_B.b_kstr_m - 1 < 8) {
15145 cartesian_waypoints_planner_B.loop_ub_n =
15146 cartesian_waypoints_planner_B.b_kstr_m - 1;
15147 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_n] !=
15148 cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.loop_ub_n])
15149 {
15150 exitg1 = 1;
15151 } else {
15152 cartesian_waypoints_planner_B.b_kstr_m++;
15153 }
15154 } else {
15155 cartesian_waypoints_planner_B.b_bool_nh = true;
15156 exitg1 = 1;
15157 }
15158 } while (exitg1 == 0);
15159 }
15160
15161 if (cartesian_waypoints_planner_B.b_bool_nh) {
15162 cartesian_waypoints_planner_B.b_kstr_m = 0;
15163 } else {
15164 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15165 cartesian_waypoints_planner_B.b_kstr_m < 9;
15166 cartesian_waypoints_planner_B.b_kstr_m++) {
15167 cartesian_waypoints_planner_B.b_c[cartesian_waypoints_planner_B.b_kstr_m] =
15168 tmp_4[cartesian_waypoints_planner_B.b_kstr_m];
15169 }
15170
15171 cartesian_waypoints_planner_B.b_bool_nh = false;
15172 if (switch_expression->size[1] == 9) {
15173 cartesian_waypoints_planner_B.b_kstr_m = 1;
15174 do {
15175 exitg1 = 0;
15176 if (cartesian_waypoints_planner_B.b_kstr_m - 1 < 9) {
15177 cartesian_waypoints_planner_B.loop_ub_n =
15178 cartesian_waypoints_planner_B.b_kstr_m - 1;
15179 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_n]
15180 !=
15181 cartesian_waypoints_planner_B.b_c[cartesian_waypoints_planner_B.loop_ub_n])
15182 {
15183 exitg1 = 1;
15184 } else {
15185 cartesian_waypoints_planner_B.b_kstr_m++;
15186 }
15187 } else {
15188 cartesian_waypoints_planner_B.b_bool_nh = true;
15189 exitg1 = 1;
15190 }
15191 } while (exitg1 == 0);
15192 }
15193
15194 if (cartesian_waypoints_planner_B.b_bool_nh) {
15195 cartesian_waypoints_planner_B.b_kstr_m = 1;
15196 } else {
15197 cartesian_waypoints_planner_B.b_kstr_m = -1;
15198 }
15199 }
15200
15201 switch (cartesian_waypoints_planner_B.b_kstr_m) {
15202 case 0:
15203 cartesian_waypoints_planner_B.iv1[0] = 0;
15204 cartesian_waypoints_planner_B.iv1[1] = 0;
15205 cartesian_waypoints_planner_B.iv1[2] = 1;
15206 cartesian_waypoints_planner_B.iv1[3] = 0;
15207 cartesian_waypoints_planner_B.iv1[4] = 0;
15208 cartesian_waypoints_planner_B.iv1[5] = 0;
15209 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15210 cartesian_waypoints_planner_B.b_kstr_m < 6;
15211 cartesian_waypoints_planner_B.b_kstr_m++) {
15212 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m]
15213 =
15214 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_m];
15215 }
15216
15217 cartesian_waypoints_planner_B.poslim_data_p[0] = -3.1415926535897931;
15218 cartesian_waypoints_planner_B.poslim_data_p[1] = 3.1415926535897931;
15219 iobj_12->VelocityNumber = 1.0;
15220 iobj_12->PositionNumber = 1.0;
15221 iobj_12->JointAxisInternal[0] = 0.0;
15222 iobj_12->JointAxisInternal[1] = 0.0;
15223 iobj_12->JointAxisInternal[2] = 1.0;
15224 break;
15225
15226 case 1:
15227 cartesian_waypoints_planner_B.iv1[0] = 0;
15228 cartesian_waypoints_planner_B.iv1[1] = 0;
15229 cartesian_waypoints_planner_B.iv1[2] = 0;
15230 cartesian_waypoints_planner_B.iv1[3] = 0;
15231 cartesian_waypoints_planner_B.iv1[4] = 0;
15232 cartesian_waypoints_planner_B.iv1[5] = 1;
15233 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15234 cartesian_waypoints_planner_B.b_kstr_m < 6;
15235 cartesian_waypoints_planner_B.b_kstr_m++) {
15236 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m]
15237 =
15238 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_m];
15239 }
15240
15241 cartesian_waypoints_planner_B.poslim_data_p[0] = -0.5;
15242 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.5;
15243 iobj_12->VelocityNumber = 1.0;
15244 iobj_12->PositionNumber = 1.0;
15245 iobj_12->JointAxisInternal[0] = 0.0;
15246 iobj_12->JointAxisInternal[1] = 0.0;
15247 iobj_12->JointAxisInternal[2] = 1.0;
15248 break;
15249
15250 default:
15251 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15252 cartesian_waypoints_planner_B.b_kstr_m < 6;
15253 cartesian_waypoints_planner_B.b_kstr_m++) {
15254 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m]
15255 = 0;
15256 }
15257
15258 cartesian_waypoints_planner_B.poslim_data_p[0] = 0.0;
15259 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.0;
15260 iobj_12->VelocityNumber = 0.0;
15261 iobj_12->PositionNumber = 0.0;
15262 iobj_12->JointAxisInternal[0] = 0.0;
15263 iobj_12->JointAxisInternal[1] = 0.0;
15264 iobj_12->JointAxisInternal[2] = 0.0;
15265 break;
15266 }
15267
15268 cartesian_waypoints_planner_B.b_kstr_m = iobj_12->MotionSubspace->size[0] *
15269 iobj_12->MotionSubspace->size[1];
15270 iobj_12->MotionSubspace->size[0] = 6;
15271 iobj_12->MotionSubspace->size[1] = 1;
15272 cartes_emxEnsureCapacity_real_T(iobj_12->MotionSubspace,
15273 cartesian_waypoints_planner_B.b_kstr_m);
15274 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15275 cartesian_waypoints_planner_B.b_kstr_m < 6;
15276 cartesian_waypoints_planner_B.b_kstr_m++) {
15277 iobj_12->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_m] =
15278 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m];
15279 }
15280
15281 cartesian_waypoints_planner_B.b_kstr_m = iobj_12->PositionLimitsInternal->
15282 size[0] * iobj_12->PositionLimitsInternal->size[1];
15283 iobj_12->PositionLimitsInternal->size[0] = 1;
15284 iobj_12->PositionLimitsInternal->size[1] = 2;
15285 cartes_emxEnsureCapacity_real_T(iobj_12->PositionLimitsInternal,
15286 cartesian_waypoints_planner_B.b_kstr_m);
15287 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15288 cartesian_waypoints_planner_B.b_kstr_m < 2;
15289 cartesian_waypoints_planner_B.b_kstr_m++) {
15290 iobj_12->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_m]
15291 =
15292 cartesian_waypoints_planner_B.poslim_data_p[cartesian_waypoints_planner_B.b_kstr_m];
15293 }
15294
15295 cartesian_waypoints_planner_B.b_kstr_m = iobj_12->HomePositionInternal->size[0];
15296 iobj_12->HomePositionInternal->size[0] = 1;
15297 cartes_emxEnsureCapacity_real_T(iobj_12->HomePositionInternal,
15298 cartesian_waypoints_planner_B.b_kstr_m);
15299 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15300 cartesian_waypoints_planner_B.b_kstr_m < 1;
15301 cartesian_waypoints_planner_B.b_kstr_m++) {
15302 iobj_12->HomePositionInternal->data[0] = 0.0;
15303 }
15304
15305 iobj_5->JointInternal = iobj_12;
15306 iobj_5->Index = -1.0;
15307 iobj_5->ParentIndex = -1.0;
15308 obj->Bodies[6] = iobj_5;
15309 cartesian_waypoints_planner_B.b_kstr_m = iobj_6->NameInternal->size[0] *
15310 iobj_6->NameInternal->size[1];
15311 iobj_6->NameInternal->size[0] = 1;
15312 iobj_6->NameInternal->size[1] = 10;
15313 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal,
15314 cartesian_waypoints_planner_B.b_kstr_m);
15315 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15316 cartesian_waypoints_planner_B.b_kstr_m < 10;
15317 cartesian_waypoints_planner_B.b_kstr_m++) {
15318 iobj_6->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_m] =
15319 tmp_7[cartesian_waypoints_planner_B.b_kstr_m];
15320 }
15321
15322 iobj_13->InTree = false;
15323 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15324 cartesian_waypoints_planner_B.b_kstr_m < 16;
15325 cartesian_waypoints_planner_B.b_kstr_m++) {
15326 iobj_13->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_m] =
15327 tmp_0[cartesian_waypoints_planner_B.b_kstr_m];
15328 }
15329
15330 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15331 cartesian_waypoints_planner_B.b_kstr_m < 16;
15332 cartesian_waypoints_planner_B.b_kstr_m++) {
15333 iobj_13->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_m] =
15334 tmp_0[cartesian_waypoints_planner_B.b_kstr_m];
15335 }
15336
15337 cartesian_waypoints_planner_B.b_kstr_m = iobj_13->NameInternal->size[0] *
15338 iobj_13->NameInternal->size[1];
15339 iobj_13->NameInternal->size[0] = 1;
15340 iobj_13->NameInternal->size[1] = 14;
15341 cartes_emxEnsureCapacity_char_T(iobj_13->NameInternal,
15342 cartesian_waypoints_planner_B.b_kstr_m);
15343 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15344 cartesian_waypoints_planner_B.b_kstr_m < 14;
15345 cartesian_waypoints_planner_B.b_kstr_m++) {
15346 iobj_13->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_m] =
15347 tmp_8[cartesian_waypoints_planner_B.b_kstr_m];
15348 }
15349
15350 cartesian_waypoints_planner_B.b_kstr_m = iobj_13->Type->size[0] *
15351 iobj_13->Type->size[1];
15352 iobj_13->Type->size[0] = 1;
15353 iobj_13->Type->size[1] = 5;
15354 cartes_emxEnsureCapacity_char_T(iobj_13->Type,
15355 cartesian_waypoints_planner_B.b_kstr_m);
15356 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15357 cartesian_waypoints_planner_B.b_kstr_m < 5;
15358 cartesian_waypoints_planner_B.b_kstr_m++) {
15359 iobj_13->Type->data[cartesian_waypoints_planner_B.b_kstr_m] =
15360 tmp_2[cartesian_waypoints_planner_B.b_kstr_m];
15361 }
15362
15363 cartesian_waypoints_planner_B.b_kstr_m = switch_expression->size[0] *
15364 switch_expression->size[1];
15365 switch_expression->size[0] = 1;
15366 switch_expression->size[1] = iobj_13->Type->size[1];
15367 cartes_emxEnsureCapacity_char_T(switch_expression,
15368 cartesian_waypoints_planner_B.b_kstr_m);
15369 cartesian_waypoints_planner_B.loop_ub_n = iobj_13->Type->size[0] *
15370 iobj_13->Type->size[1] - 1;
15371 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15372 cartesian_waypoints_planner_B.b_kstr_m <=
15373 cartesian_waypoints_planner_B.loop_ub_n;
15374 cartesian_waypoints_planner_B.b_kstr_m++) {
15375 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_m] =
15376 iobj_13->Type->data[cartesian_waypoints_planner_B.b_kstr_m];
15377 }
15378
15379 cartesian_waypoints_planner_B.b_bool_nh = false;
15380 if (switch_expression->size[1] == 8) {
15381 cartesian_waypoints_planner_B.b_kstr_m = 1;
15382 do {
15383 exitg1 = 0;
15384 if (cartesian_waypoints_planner_B.b_kstr_m - 1 < 8) {
15385 cartesian_waypoints_planner_B.loop_ub_n =
15386 cartesian_waypoints_planner_B.b_kstr_m - 1;
15387 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_n] !=
15388 cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.loop_ub_n])
15389 {
15390 exitg1 = 1;
15391 } else {
15392 cartesian_waypoints_planner_B.b_kstr_m++;
15393 }
15394 } else {
15395 cartesian_waypoints_planner_B.b_bool_nh = true;
15396 exitg1 = 1;
15397 }
15398 } while (exitg1 == 0);
15399 }
15400
15401 if (cartesian_waypoints_planner_B.b_bool_nh) {
15402 cartesian_waypoints_planner_B.b_kstr_m = 0;
15403 } else {
15404 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15405 cartesian_waypoints_planner_B.b_kstr_m < 9;
15406 cartesian_waypoints_planner_B.b_kstr_m++) {
15407 cartesian_waypoints_planner_B.b_c[cartesian_waypoints_planner_B.b_kstr_m] =
15408 tmp_4[cartesian_waypoints_planner_B.b_kstr_m];
15409 }
15410
15411 cartesian_waypoints_planner_B.b_bool_nh = false;
15412 if (switch_expression->size[1] == 9) {
15413 cartesian_waypoints_planner_B.b_kstr_m = 1;
15414 do {
15415 exitg1 = 0;
15416 if (cartesian_waypoints_planner_B.b_kstr_m - 1 < 9) {
15417 cartesian_waypoints_planner_B.loop_ub_n =
15418 cartesian_waypoints_planner_B.b_kstr_m - 1;
15419 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_n]
15420 !=
15421 cartesian_waypoints_planner_B.b_c[cartesian_waypoints_planner_B.loop_ub_n])
15422 {
15423 exitg1 = 1;
15424 } else {
15425 cartesian_waypoints_planner_B.b_kstr_m++;
15426 }
15427 } else {
15428 cartesian_waypoints_planner_B.b_bool_nh = true;
15429 exitg1 = 1;
15430 }
15431 } while (exitg1 == 0);
15432 }
15433
15434 if (cartesian_waypoints_planner_B.b_bool_nh) {
15435 cartesian_waypoints_planner_B.b_kstr_m = 1;
15436 } else {
15437 cartesian_waypoints_planner_B.b_kstr_m = -1;
15438 }
15439 }
15440
15441 cartesian_waypoi_emxFree_char_T(&switch_expression);
15442 switch (cartesian_waypoints_planner_B.b_kstr_m) {
15443 case 0:
15444 cartesian_waypoints_planner_B.iv1[0] = 0;
15445 cartesian_waypoints_planner_B.iv1[1] = 0;
15446 cartesian_waypoints_planner_B.iv1[2] = 1;
15447 cartesian_waypoints_planner_B.iv1[3] = 0;
15448 cartesian_waypoints_planner_B.iv1[4] = 0;
15449 cartesian_waypoints_planner_B.iv1[5] = 0;
15450 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15451 cartesian_waypoints_planner_B.b_kstr_m < 6;
15452 cartesian_waypoints_planner_B.b_kstr_m++) {
15453 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m]
15454 =
15455 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_m];
15456 }
15457
15458 cartesian_waypoints_planner_B.poslim_data_p[0] = -3.1415926535897931;
15459 cartesian_waypoints_planner_B.poslim_data_p[1] = 3.1415926535897931;
15460 iobj_13->VelocityNumber = 1.0;
15461 iobj_13->PositionNumber = 1.0;
15462 iobj_13->JointAxisInternal[0] = 0.0;
15463 iobj_13->JointAxisInternal[1] = 0.0;
15464 iobj_13->JointAxisInternal[2] = 1.0;
15465 break;
15466
15467 case 1:
15468 cartesian_waypoints_planner_B.iv1[0] = 0;
15469 cartesian_waypoints_planner_B.iv1[1] = 0;
15470 cartesian_waypoints_planner_B.iv1[2] = 0;
15471 cartesian_waypoints_planner_B.iv1[3] = 0;
15472 cartesian_waypoints_planner_B.iv1[4] = 0;
15473 cartesian_waypoints_planner_B.iv1[5] = 1;
15474 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15475 cartesian_waypoints_planner_B.b_kstr_m < 6;
15476 cartesian_waypoints_planner_B.b_kstr_m++) {
15477 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m]
15478 =
15479 cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_m];
15480 }
15481
15482 cartesian_waypoints_planner_B.poslim_data_p[0] = -0.5;
15483 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.5;
15484 iobj_13->VelocityNumber = 1.0;
15485 iobj_13->PositionNumber = 1.0;
15486 iobj_13->JointAxisInternal[0] = 0.0;
15487 iobj_13->JointAxisInternal[1] = 0.0;
15488 iobj_13->JointAxisInternal[2] = 1.0;
15489 break;
15490
15491 default:
15492 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15493 cartesian_waypoints_planner_B.b_kstr_m < 6;
15494 cartesian_waypoints_planner_B.b_kstr_m++) {
15495 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m]
15496 = 0;
15497 }
15498
15499 cartesian_waypoints_planner_B.poslim_data_p[0] = 0.0;
15500 cartesian_waypoints_planner_B.poslim_data_p[1] = 0.0;
15501 iobj_13->VelocityNumber = 0.0;
15502 iobj_13->PositionNumber = 0.0;
15503 iobj_13->JointAxisInternal[0] = 0.0;
15504 iobj_13->JointAxisInternal[1] = 0.0;
15505 iobj_13->JointAxisInternal[2] = 0.0;
15506 break;
15507 }
15508
15509 cartesian_waypoints_planner_B.b_kstr_m = iobj_13->MotionSubspace->size[0] *
15510 iobj_13->MotionSubspace->size[1];
15511 iobj_13->MotionSubspace->size[0] = 6;
15512 iobj_13->MotionSubspace->size[1] = 1;
15513 cartes_emxEnsureCapacity_real_T(iobj_13->MotionSubspace,
15514 cartesian_waypoints_planner_B.b_kstr_m);
15515 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15516 cartesian_waypoints_planner_B.b_kstr_m < 6;
15517 cartesian_waypoints_planner_B.b_kstr_m++) {
15518 iobj_13->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_m] =
15519 cartesian_waypoints_planner_B.msubspace_data_l[cartesian_waypoints_planner_B.b_kstr_m];
15520 }
15521
15522 cartesian_waypoints_planner_B.b_kstr_m = iobj_13->PositionLimitsInternal->
15523 size[0] * iobj_13->PositionLimitsInternal->size[1];
15524 iobj_13->PositionLimitsInternal->size[0] = 1;
15525 iobj_13->PositionLimitsInternal->size[1] = 2;
15526 cartes_emxEnsureCapacity_real_T(iobj_13->PositionLimitsInternal,
15527 cartesian_waypoints_planner_B.b_kstr_m);
15528 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15529 cartesian_waypoints_planner_B.b_kstr_m < 2;
15530 cartesian_waypoints_planner_B.b_kstr_m++) {
15531 iobj_13->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_m]
15532 =
15533 cartesian_waypoints_planner_B.poslim_data_p[cartesian_waypoints_planner_B.b_kstr_m];
15534 }
15535
15536 cartesian_waypoints_planner_B.b_kstr_m = iobj_13->HomePositionInternal->size[0];
15537 iobj_13->HomePositionInternal->size[0] = 1;
15538 cartes_emxEnsureCapacity_real_T(iobj_13->HomePositionInternal,
15539 cartesian_waypoints_planner_B.b_kstr_m);
15540 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15541 cartesian_waypoints_planner_B.b_kstr_m < 1;
15542 cartesian_waypoints_planner_B.b_kstr_m++) {
15543 iobj_13->HomePositionInternal->data[0] = 0.0;
15544 }
15545
15546 iobj_6->JointInternal = iobj_13;
15547 iobj_6->Index = -1.0;
15548 iobj_6->ParentIndex = -1.0;
15549 obj->Bodies[7] = iobj_6;
15550 obj->NumBodies = 0.0;
15551 obj->NumNonFixedBodies = 0.0;
15552 obj->PositionNumber = 0.0;
15553 obj->VelocityNumber = 0.0;
15554 cartesian_waypoints_planne_rand(cartesian_waypoints_planner_B.unusedExpr_n);
15555 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15556 cartesian_waypoints_planner_B.b_kstr_m < 8;
15557 cartesian_waypoints_planner_B.b_kstr_m++) {
15558 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_m] = 0.0;
15559 }
15560
15561 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15562 cartesian_waypoints_planner_B.b_kstr_m < 8;
15563 cartesian_waypoints_planner_B.b_kstr_m++) {
15564 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_m + 8] = -1.0;
15565 }
15566
15567 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15568 cartesian_waypoints_planner_B.b_kstr_m < 8;
15569 cartesian_waypoints_planner_B.b_kstr_m++) {
15570 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_m] = 0.0;
15571 }
15572
15573 for (cartesian_waypoints_planner_B.b_kstr_m = 0;
15574 cartesian_waypoints_planner_B.b_kstr_m < 8;
15575 cartesian_waypoints_planner_B.b_kstr_m++) {
15576 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_m + 8] = -1.0;
15577 }
15578}
15579
15580static x_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_ev
15581 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
15582 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
15583 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
15584 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
15585 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
15586 c_rigidBodyJoint_cartesian__e_T *iobj_7, c_rigidBodyJoint_cartesian__e_T
15587 *iobj_8, c_rigidBodyJoint_cartesian__e_T *iobj_9,
15588 c_rigidBodyJoint_cartesian__e_T *iobj_10, c_rigidBodyJoint_cartesian__e_T
15589 *iobj_11, c_rigidBodyJoint_cartesian__e_T *iobj_12,
15590 c_rigidBodyJoint_cartesian__e_T *iobj_13, c_rigidBodyJoint_cartesian__e_T
15591 *iobj_14, c_rigidBodyJoint_cartesian__e_T *iobj_15,
15592 w_robotics_manip_internal_Rig_T *iobj_16)
15593{
15594 x_robotics_manip_internal_Rig_T *b_obj;
15595 emxArray_char_T_cartesian_way_T *switch_expression;
15596 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
15597 };
15598
15599 static const char_T tmp_0[8] = { 'b', 'a', 's', 'e', '_', 'j', 'n', 't' };
15600
15601 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
15602
15603 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15604
15605 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15606
15607 int32_T exitg1;
15608 b_obj = obj;
15609 cartesian_waypoints_planner_B.b_kstr_g = obj->Base.NameInternal->size[0] *
15610 obj->Base.NameInternal->size[1];
15611 obj->Base.NameInternal->size[0] = 1;
15612 obj->Base.NameInternal->size[1] = 4;
15613 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal,
15614 cartesian_waypoints_planner_B.b_kstr_g);
15615 obj->Base.NameInternal->data[0] = 'b';
15616 obj->Base.NameInternal->data[1] = 'a';
15617 obj->Base.NameInternal->data[2] = 's';
15618 obj->Base.NameInternal->data[3] = 'e';
15619 iobj_15->InTree = false;
15620 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15621 cartesian_waypoints_planner_B.b_kstr_g < 16;
15622 cartesian_waypoints_planner_B.b_kstr_g++) {
15623 iobj_15->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_g] =
15624 tmp[cartesian_waypoints_planner_B.b_kstr_g];
15625 }
15626
15627 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15628 cartesian_waypoints_planner_B.b_kstr_g < 16;
15629 cartesian_waypoints_planner_B.b_kstr_g++) {
15630 iobj_15->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_g] =
15631 tmp[cartesian_waypoints_planner_B.b_kstr_g];
15632 }
15633
15634 cartesian_waypoints_planner_B.b_kstr_g = iobj_15->NameInternal->size[0] *
15635 iobj_15->NameInternal->size[1];
15636 iobj_15->NameInternal->size[0] = 1;
15637 iobj_15->NameInternal->size[1] = 8;
15638 cartes_emxEnsureCapacity_char_T(iobj_15->NameInternal,
15639 cartesian_waypoints_planner_B.b_kstr_g);
15640 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15641 cartesian_waypoints_planner_B.b_kstr_g < 8;
15642 cartesian_waypoints_planner_B.b_kstr_g++) {
15643 iobj_15->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_g] =
15644 tmp_0[cartesian_waypoints_planner_B.b_kstr_g];
15645 }
15646
15647 cartesian_waypoints_planner_B.b_kstr_g = iobj_15->Type->size[0] *
15648 iobj_15->Type->size[1];
15649 iobj_15->Type->size[0] = 1;
15650 iobj_15->Type->size[1] = 5;
15651 cartes_emxEnsureCapacity_char_T(iobj_15->Type,
15652 cartesian_waypoints_planner_B.b_kstr_g);
15653 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15654 cartesian_waypoints_planner_B.b_kstr_g < 5;
15655 cartesian_waypoints_planner_B.b_kstr_g++) {
15656 iobj_15->Type->data[cartesian_waypoints_planner_B.b_kstr_g] =
15657 tmp_1[cartesian_waypoints_planner_B.b_kstr_g];
15658 }
15659
15660 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
15661 cartesian_waypoints_planner_B.b_kstr_g = switch_expression->size[0] *
15662 switch_expression->size[1];
15663 switch_expression->size[0] = 1;
15664 switch_expression->size[1] = iobj_15->Type->size[1];
15665 cartes_emxEnsureCapacity_char_T(switch_expression,
15666 cartesian_waypoints_planner_B.b_kstr_g);
15667 cartesian_waypoints_planner_B.loop_ub_l = iobj_15->Type->size[0] *
15668 iobj_15->Type->size[1] - 1;
15669 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15670 cartesian_waypoints_planner_B.b_kstr_g <=
15671 cartesian_waypoints_planner_B.loop_ub_l;
15672 cartesian_waypoints_planner_B.b_kstr_g++) {
15673 switch_expression->data[cartesian_waypoints_planner_B.b_kstr_g] =
15674 iobj_15->Type->data[cartesian_waypoints_planner_B.b_kstr_g];
15675 }
15676
15677 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15678 cartesian_waypoints_planner_B.b_kstr_g < 8;
15679 cartesian_waypoints_planner_B.b_kstr_g++) {
15680 cartesian_waypoints_planner_B.b_m0[cartesian_waypoints_planner_B.b_kstr_g] =
15681 tmp_2[cartesian_waypoints_planner_B.b_kstr_g];
15682 }
15683
15684 cartesian_waypoints_planner_B.b_bool_g = false;
15685 if (switch_expression->size[1] == 8) {
15686 cartesian_waypoints_planner_B.b_kstr_g = 1;
15687 do {
15688 exitg1 = 0;
15689 if (cartesian_waypoints_planner_B.b_kstr_g - 1 < 8) {
15690 cartesian_waypoints_planner_B.loop_ub_l =
15691 cartesian_waypoints_planner_B.b_kstr_g - 1;
15692 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_l] !=
15693 cartesian_waypoints_planner_B.b_m0[cartesian_waypoints_planner_B.loop_ub_l])
15694 {
15695 exitg1 = 1;
15696 } else {
15697 cartesian_waypoints_planner_B.b_kstr_g++;
15698 }
15699 } else {
15700 cartesian_waypoints_planner_B.b_bool_g = true;
15701 exitg1 = 1;
15702 }
15703 } while (exitg1 == 0);
15704 }
15705
15706 if (cartesian_waypoints_planner_B.b_bool_g) {
15707 cartesian_waypoints_planner_B.b_kstr_g = 0;
15708 } else {
15709 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15710 cartesian_waypoints_planner_B.b_kstr_g < 9;
15711 cartesian_waypoints_planner_B.b_kstr_g++) {
15712 cartesian_waypoints_planner_B.b_b[cartesian_waypoints_planner_B.b_kstr_g] =
15713 tmp_3[cartesian_waypoints_planner_B.b_kstr_g];
15714 }
15715
15716 cartesian_waypoints_planner_B.b_bool_g = false;
15717 if (switch_expression->size[1] == 9) {
15718 cartesian_waypoints_planner_B.b_kstr_g = 1;
15719 do {
15720 exitg1 = 0;
15721 if (cartesian_waypoints_planner_B.b_kstr_g - 1 < 9) {
15722 cartesian_waypoints_planner_B.loop_ub_l =
15723 cartesian_waypoints_planner_B.b_kstr_g - 1;
15724 if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_l]
15725 !=
15726 cartesian_waypoints_planner_B.b_b[cartesian_waypoints_planner_B.loop_ub_l])
15727 {
15728 exitg1 = 1;
15729 } else {
15730 cartesian_waypoints_planner_B.b_kstr_g++;
15731 }
15732 } else {
15733 cartesian_waypoints_planner_B.b_bool_g = true;
15734 exitg1 = 1;
15735 }
15736 } while (exitg1 == 0);
15737 }
15738
15739 if (cartesian_waypoints_planner_B.b_bool_g) {
15740 cartesian_waypoints_planner_B.b_kstr_g = 1;
15741 } else {
15742 cartesian_waypoints_planner_B.b_kstr_g = -1;
15743 }
15744 }
15745
15746 cartesian_waypoi_emxFree_char_T(&switch_expression);
15747 switch (cartesian_waypoints_planner_B.b_kstr_g) {
15748 case 0:
15749 cartesian_waypoints_planner_B.iv[0] = 0;
15750 cartesian_waypoints_planner_B.iv[1] = 0;
15751 cartesian_waypoints_planner_B.iv[2] = 1;
15752 cartesian_waypoints_planner_B.iv[3] = 0;
15753 cartesian_waypoints_planner_B.iv[4] = 0;
15754 cartesian_waypoints_planner_B.iv[5] = 0;
15755 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15756 cartesian_waypoints_planner_B.b_kstr_g < 6;
15757 cartesian_waypoints_planner_B.b_kstr_g++) {
15758 cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_g]
15759 =
15760 cartesian_waypoints_planner_B.iv[cartesian_waypoints_planner_B.b_kstr_g];
15761 }
15762
15763 cartesian_waypoints_planner_B.poslim_data[0] = -3.1415926535897931;
15764 cartesian_waypoints_planner_B.poslim_data[1] = 3.1415926535897931;
15765 iobj_15->VelocityNumber = 1.0;
15766 iobj_15->PositionNumber = 1.0;
15767 iobj_15->JointAxisInternal[0] = 0.0;
15768 iobj_15->JointAxisInternal[1] = 0.0;
15769 iobj_15->JointAxisInternal[2] = 1.0;
15770 break;
15771
15772 case 1:
15773 cartesian_waypoints_planner_B.iv[0] = 0;
15774 cartesian_waypoints_planner_B.iv[1] = 0;
15775 cartesian_waypoints_planner_B.iv[2] = 0;
15776 cartesian_waypoints_planner_B.iv[3] = 0;
15777 cartesian_waypoints_planner_B.iv[4] = 0;
15778 cartesian_waypoints_planner_B.iv[5] = 1;
15779 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15780 cartesian_waypoints_planner_B.b_kstr_g < 6;
15781 cartesian_waypoints_planner_B.b_kstr_g++) {
15782 cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_g]
15783 =
15784 cartesian_waypoints_planner_B.iv[cartesian_waypoints_planner_B.b_kstr_g];
15785 }
15786
15787 cartesian_waypoints_planner_B.poslim_data[0] = -0.5;
15788 cartesian_waypoints_planner_B.poslim_data[1] = 0.5;
15789 iobj_15->VelocityNumber = 1.0;
15790 iobj_15->PositionNumber = 1.0;
15791 iobj_15->JointAxisInternal[0] = 0.0;
15792 iobj_15->JointAxisInternal[1] = 0.0;
15793 iobj_15->JointAxisInternal[2] = 1.0;
15794 break;
15795
15796 default:
15797 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15798 cartesian_waypoints_planner_B.b_kstr_g < 6;
15799 cartesian_waypoints_planner_B.b_kstr_g++) {
15800 cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_g]
15801 = 0;
15802 }
15803
15804 cartesian_waypoints_planner_B.poslim_data[0] = 0.0;
15805 cartesian_waypoints_planner_B.poslim_data[1] = 0.0;
15806 iobj_15->VelocityNumber = 0.0;
15807 iobj_15->PositionNumber = 0.0;
15808 iobj_15->JointAxisInternal[0] = 0.0;
15809 iobj_15->JointAxisInternal[1] = 0.0;
15810 iobj_15->JointAxisInternal[2] = 0.0;
15811 break;
15812 }
15813
15814 cartesian_waypoints_planner_B.b_kstr_g = iobj_15->MotionSubspace->size[0] *
15815 iobj_15->MotionSubspace->size[1];
15816 iobj_15->MotionSubspace->size[0] = 6;
15817 iobj_15->MotionSubspace->size[1] = 1;
15818 cartes_emxEnsureCapacity_real_T(iobj_15->MotionSubspace,
15819 cartesian_waypoints_planner_B.b_kstr_g);
15820 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15821 cartesian_waypoints_planner_B.b_kstr_g < 6;
15822 cartesian_waypoints_planner_B.b_kstr_g++) {
15823 iobj_15->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_g] =
15824 cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_g];
15825 }
15826
15827 cartesian_waypoints_planner_B.b_kstr_g = iobj_15->PositionLimitsInternal->
15828 size[0] * iobj_15->PositionLimitsInternal->size[1];
15829 iobj_15->PositionLimitsInternal->size[0] = 1;
15830 iobj_15->PositionLimitsInternal->size[1] = 2;
15831 cartes_emxEnsureCapacity_real_T(iobj_15->PositionLimitsInternal,
15832 cartesian_waypoints_planner_B.b_kstr_g);
15833 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15834 cartesian_waypoints_planner_B.b_kstr_g < 2;
15835 cartesian_waypoints_planner_B.b_kstr_g++) {
15836 iobj_15->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_g]
15837 =
15838 cartesian_waypoints_planner_B.poslim_data[cartesian_waypoints_planner_B.b_kstr_g];
15839 }
15840
15841 cartesian_waypoints_planner_B.b_kstr_g = iobj_15->HomePositionInternal->size[0];
15842 iobj_15->HomePositionInternal->size[0] = 1;
15843 cartes_emxEnsureCapacity_real_T(iobj_15->HomePositionInternal,
15844 cartesian_waypoints_planner_B.b_kstr_g);
15845 for (cartesian_waypoints_planner_B.b_kstr_g = 0;
15846 cartesian_waypoints_planner_B.b_kstr_g < 1;
15847 cartesian_waypoints_planner_B.b_kstr_g++) {
15848 iobj_15->HomePositionInternal->data[0] = 0.0;
15849 }
15850
15851 obj->Base.JointInternal = iobj_15;
15852 obj->Base.Index = -1.0;
15853 obj->Base.ParentIndex = -1.0;
15854 obj->Base.Index = 0.0;
15855 cartesian_waypoints_planne_rand(cartesian_waypoints_planner_B.unusedExpr);
15856 ca_RigidBodyTree_clearAllBodies(obj, iobj_0, iobj_1, iobj_2, iobj_3, iobj_4,
15857 iobj_5, iobj_6, iobj_8, iobj_9, iobj_10, iobj_11, iobj_12, iobj_13, iobj_14,
15858 iobj_7, iobj_16);
15859 return b_obj;
15860}
15861
15862static c_rigidBodyJoint_cartesian__e_T *c_rigidBodyJoint_rigidBodyJoint
15863 (c_rigidBodyJoint_cartesian__e_T *obj, const emxArray_char_T_cartesian_way_T
15864 *jname, const emxArray_char_T_cartesian_way_T *jtype)
15865{
15866 c_rigidBodyJoint_cartesian__e_T *b_obj;
15867 emxArray_char_T_cartesian_way_T *switch_expression;
15868 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
15869 };
15870
15871 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15872
15873 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15874
15875 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
15876
15877 static const char_T tmp_3[128] = { '\x00', '\x01', '\x02', '\x03', '\x04',
15878 '\x05', '\x06', '\x07', '\x08', '\x09', '\x0a', '\x0b', '\x0c', '\x0d',
15879 '\x0e', '\x0f', '\x10', '\x11', '\x12', '\x13', '\x14', '\x15', '\x16',
15880 '\x17', '\x18', '\x19', '\x1a', '\x1b', '\x1c', '\x1d', '\x1e', '\x1f', ' ',
15881 '!', '\"', '#', '$', '%', '&', '\'', '(', ')', '*', '+', ',', '-', '.', '/',
15882 '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', ':', ';', '<', '=', '>',
15883 '?', '@', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm',
15884 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', '[', '\\',
15885 ']', '^', '_', '`', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k',
15886 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z',
15887 '{', '|', '}', '~', '\x7f' };
15888
15889 boolean_T guard1 = false;
15890 boolean_T guard2 = false;
15891 boolean_T guard3 = false;
15892 boolean_T guard4 = false;
15893 int32_T exitg1;
15894 boolean_T guard11 = false;
15895 obj->InTree = false;
15896 for (cartesian_waypoints_planner_B.minnanb = 0;
15897 cartesian_waypoints_planner_B.minnanb < 16;
15898 cartesian_waypoints_planner_B.minnanb++) {
15899 obj->JointToParentTransform[cartesian_waypoints_planner_B.minnanb] =
15900 tmp[cartesian_waypoints_planner_B.minnanb];
15901 }
15902
15903 for (cartesian_waypoints_planner_B.minnanb = 0;
15904 cartesian_waypoints_planner_B.minnanb < 16;
15905 cartesian_waypoints_planner_B.minnanb++) {
15906 obj->ChildToJointTransform[cartesian_waypoints_planner_B.minnanb] =
15907 tmp[cartesian_waypoints_planner_B.minnanb];
15908 }
15909
15910 b_obj = obj;
15911 cartesian_waypoints_planner_B.minnanb = obj->NameInternal->size[0] *
15912 obj->NameInternal->size[1];
15913 obj->NameInternal->size[0] = 1;
15914 obj->NameInternal->size[1] = jname->size[1];
15915 cartes_emxEnsureCapacity_char_T(obj->NameInternal,
15916 cartesian_waypoints_planner_B.minnanb);
15917 cartesian_waypoints_planner_B.loop_ub_pd = jname->size[0] * jname->size[1] - 1;
15918 for (cartesian_waypoints_planner_B.minnanb = 0;
15919 cartesian_waypoints_planner_B.minnanb <=
15920 cartesian_waypoints_planner_B.loop_ub_pd;
15921 cartesian_waypoints_planner_B.minnanb++) {
15922 obj->NameInternal->data[cartesian_waypoints_planner_B.minnanb] = jname->
15923 data[cartesian_waypoints_planner_B.minnanb];
15924 }
15925
15926 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 0;
15927 for (cartesian_waypoints_planner_B.minnanb = 0;
15928 cartesian_waypoints_planner_B.minnanb < 8;
15929 cartesian_waypoints_planner_B.minnanb++) {
15930 cartesian_waypoints_planner_B.vstr[cartesian_waypoints_planner_B.minnanb] =
15931 tmp_0[cartesian_waypoints_planner_B.minnanb];
15932 }
15933
15934 guard1 = false;
15935 guard2 = false;
15936 guard3 = false;
15937 guard4 = false;
15938 if (jtype->size[1] <= 8) {
15939 cartesian_waypoints_planner_B.loop_ub_pd = jtype->size[1];
15940 for (cartesian_waypoints_planner_B.minnanb = 0;
15941 cartesian_waypoints_planner_B.minnanb < 8;
15942 cartesian_waypoints_planner_B.minnanb++) {
15943 cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.minnanb] =
15944 tmp_0[cartesian_waypoints_planner_B.minnanb];
15945 }
15946
15947 cartesian_waypoints_planner_B.b_bool_is = false;
15948 cartesian_waypoints_planner_B.minnanb = jtype->size[1];
15949 if (cartesian_waypoints_planner_B.minnanb >= 8) {
15950 cartesian_waypoints_planner_B.minnanb = 8;
15951 }
15952
15953 guard11 = false;
15954 if (cartesian_waypoints_planner_B.loop_ub_pd <=
15955 cartesian_waypoints_planner_B.minnanb) {
15956 if (cartesian_waypoints_planner_B.minnanb <
15957 cartesian_waypoints_planner_B.loop_ub_pd) {
15958 cartesian_waypoints_planner_B.loop_ub_pd =
15959 cartesian_waypoints_planner_B.minnanb;
15960 }
15961
15962 cartesian_waypoints_planner_B.minnanb =
15963 cartesian_waypoints_planner_B.loop_ub_pd - 1;
15964 guard11 = true;
15965 } else {
15966 if (jtype->size[1] == 8) {
15967 cartesian_waypoints_planner_B.minnanb = 7;
15968 guard11 = true;
15969 }
15970 }
15971
15972 if (guard11) {
15973 cartesian_waypoints_planner_B.loop_ub_pd = 1;
15974 do {
15975 exitg1 = 0;
15976 if (cartesian_waypoints_planner_B.loop_ub_pd - 1 <=
15977 cartesian_waypoints_planner_B.minnanb) {
15978 cartesian_waypoints_planner_B.kstr =
15979 cartesian_waypoints_planner_B.loop_ub_pd - 1;
15980 if (tmp_3[static_cast<uint8_T>(jtype->
15981 data[cartesian_waypoints_planner_B.kstr]) & 127] != tmp_3[
15982 static_cast<int32_T>
15983 (cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.kstr])])
15984 {
15985 exitg1 = 1;
15986 } else {
15987 cartesian_waypoints_planner_B.loop_ub_pd++;
15988 }
15989 } else {
15990 cartesian_waypoints_planner_B.b_bool_is = true;
15991 exitg1 = 1;
15992 }
15993 } while (exitg1 == 0);
15994 }
15995
15996 if (cartesian_waypoints_planner_B.b_bool_is) {
15997 if (jtype->size[1] == 8) {
15998 cartesian_waypoints_planner_B.nmatched = 1;
15999 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 8;
16000 for (cartesian_waypoints_planner_B.minnanb = 0;
16001 cartesian_waypoints_planner_B.minnanb < 8;
16002 cartesian_waypoints_planner_B.minnanb++) {
16003 cartesian_waypoints_planner_B.b_m[cartesian_waypoints_planner_B.minnanb]
16004 =
16005 cartesian_waypoints_planner_B.vstr[cartesian_waypoints_planner_B.minnanb];
16006 }
16007 } else {
16008 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 8;
16009 for (cartesian_waypoints_planner_B.minnanb = 0;
16010 cartesian_waypoints_planner_B.minnanb < 8;
16011 cartesian_waypoints_planner_B.minnanb++) {
16012 cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb]
16013 =
16014 cartesian_waypoints_planner_B.vstr[cartesian_waypoints_planner_B.minnanb];
16015 }
16016
16017 cartesian_waypoints_planner_B.matched = true;
16018 cartesian_waypoints_planner_B.nmatched = 1;
16019 guard3 = true;
16020 }
16021 } else {
16022 guard4 = true;
16023 }
16024 } else {
16025 guard4 = true;
16026 }
16027
16028 if (guard4) {
16029 cartesian_waypoints_planner_B.matched = false;
16030 cartesian_waypoints_planner_B.nmatched = 0;
16031 guard3 = true;
16032 }
16033
16034 if (guard3) {
16035 for (cartesian_waypoints_planner_B.minnanb = 0;
16036 cartesian_waypoints_planner_B.minnanb < 9;
16037 cartesian_waypoints_planner_B.minnanb++) {
16038 cartesian_waypoints_planner_B.b_vstr[cartesian_waypoints_planner_B.minnanb]
16039 = tmp_1[cartesian_waypoints_planner_B.minnanb];
16040 }
16041
16042 if (jtype->size[1] <= 9) {
16043 cartesian_waypoints_planner_B.loop_ub_pd = jtype->size[1];
16044 for (cartesian_waypoints_planner_B.minnanb = 0;
16045 cartesian_waypoints_planner_B.minnanb < 9;
16046 cartesian_waypoints_planner_B.minnanb++) {
16047 cartesian_waypoints_planner_B.b_m[cartesian_waypoints_planner_B.minnanb]
16048 = tmp_1[cartesian_waypoints_planner_B.minnanb];
16049 }
16050
16051 cartesian_waypoints_planner_B.b_bool_is = false;
16052 cartesian_waypoints_planner_B.minnanb = jtype->size[1];
16053 if (cartesian_waypoints_planner_B.minnanb >= 9) {
16054 cartesian_waypoints_planner_B.minnanb = 9;
16055 }
16056
16057 guard11 = false;
16058 if (cartesian_waypoints_planner_B.loop_ub_pd <=
16059 cartesian_waypoints_planner_B.minnanb) {
16060 if (cartesian_waypoints_planner_B.minnanb <
16061 cartesian_waypoints_planner_B.loop_ub_pd) {
16062 cartesian_waypoints_planner_B.loop_ub_pd =
16063 cartesian_waypoints_planner_B.minnanb;
16064 }
16065
16066 cartesian_waypoints_planner_B.minnanb =
16067 cartesian_waypoints_planner_B.loop_ub_pd - 1;
16068 guard11 = true;
16069 } else {
16070 if (jtype->size[1] == 9) {
16071 cartesian_waypoints_planner_B.minnanb = 8;
16072 guard11 = true;
16073 }
16074 }
16075
16076 if (guard11) {
16077 cartesian_waypoints_planner_B.loop_ub_pd = 1;
16078 do {
16079 exitg1 = 0;
16080 if (cartesian_waypoints_planner_B.loop_ub_pd - 1 <=
16081 cartesian_waypoints_planner_B.minnanb) {
16082 cartesian_waypoints_planner_B.kstr =
16083 cartesian_waypoints_planner_B.loop_ub_pd - 1;
16084 if (tmp_3[static_cast<uint8_T>(jtype->
16085 data[cartesian_waypoints_planner_B.kstr]) & 127] != tmp_3[
16086 static_cast<int32_T>
16087 (cartesian_waypoints_planner_B.b_m[cartesian_waypoints_planner_B.kstr])])
16088 {
16089 exitg1 = 1;
16090 } else {
16091 cartesian_waypoints_planner_B.loop_ub_pd++;
16092 }
16093 } else {
16094 cartesian_waypoints_planner_B.b_bool_is = true;
16095 exitg1 = 1;
16096 }
16097 } while (exitg1 == 0);
16098 }
16099
16100 if (cartesian_waypoints_planner_B.b_bool_is) {
16101 if (jtype->size[1] == 9) {
16102 cartesian_waypoints_planner_B.nmatched = 1;
16103 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 9;
16104 for (cartesian_waypoints_planner_B.minnanb = 0;
16105 cartesian_waypoints_planner_B.minnanb < 9;
16106 cartesian_waypoints_planner_B.minnanb++) {
16107 cartesian_waypoints_planner_B.b_m[cartesian_waypoints_planner_B.minnanb]
16108 =
16109 cartesian_waypoints_planner_B.b_vstr[cartesian_waypoints_planner_B.minnanb];
16110 }
16111 } else {
16112 if (!cartesian_waypoints_planner_B.matched) {
16113 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 9;
16114 for (cartesian_waypoints_planner_B.minnanb = 0;
16115 cartesian_waypoints_planner_B.minnanb < 9;
16116 cartesian_waypoints_planner_B.minnanb++) {
16117 cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb]
16118 =
16119 cartesian_waypoints_planner_B.b_vstr[cartesian_waypoints_planner_B.minnanb];
16120 }
16121 }
16122
16123 cartesian_waypoints_planner_B.matched = true;
16124 cartesian_waypoints_planner_B.nmatched++;
16125 guard2 = true;
16126 }
16127 } else {
16128 guard2 = true;
16129 }
16130 } else {
16131 guard2 = true;
16132 }
16133 }
16134
16135 if (guard2) {
16136 for (cartesian_waypoints_planner_B.minnanb = 0;
16137 cartesian_waypoints_planner_B.minnanb < 5;
16138 cartesian_waypoints_planner_B.minnanb++) {
16139 cartesian_waypoints_planner_B.c_vstr[cartesian_waypoints_planner_B.minnanb]
16140 = tmp_2[cartesian_waypoints_planner_B.minnanb];
16141 }
16142
16143 if (jtype->size[1] <= 5) {
16144 cartesian_waypoints_planner_B.loop_ub_pd = jtype->size[1];
16145 for (cartesian_waypoints_planner_B.minnanb = 0;
16146 cartesian_waypoints_planner_B.minnanb < 5;
16147 cartesian_waypoints_planner_B.minnanb++) {
16148 cartesian_waypoints_planner_B.b_jo[cartesian_waypoints_planner_B.minnanb]
16149 = tmp_2[cartesian_waypoints_planner_B.minnanb];
16150 }
16151
16152 cartesian_waypoints_planner_B.b_bool_is = false;
16153 cartesian_waypoints_planner_B.minnanb = jtype->size[1];
16154 if (cartesian_waypoints_planner_B.minnanb >= 5) {
16155 cartesian_waypoints_planner_B.minnanb = 5;
16156 }
16157
16158 guard11 = false;
16159 if (cartesian_waypoints_planner_B.loop_ub_pd <=
16160 cartesian_waypoints_planner_B.minnanb) {
16161 if (cartesian_waypoints_planner_B.minnanb <
16162 cartesian_waypoints_planner_B.loop_ub_pd) {
16163 cartesian_waypoints_planner_B.loop_ub_pd =
16164 cartesian_waypoints_planner_B.minnanb;
16165 }
16166
16167 cartesian_waypoints_planner_B.minnanb =
16168 cartesian_waypoints_planner_B.loop_ub_pd - 1;
16169 guard11 = true;
16170 } else {
16171 if (jtype->size[1] == 5) {
16172 cartesian_waypoints_planner_B.minnanb = 4;
16173 guard11 = true;
16174 }
16175 }
16176
16177 if (guard11) {
16178 cartesian_waypoints_planner_B.loop_ub_pd = 1;
16179 do {
16180 exitg1 = 0;
16181 if (cartesian_waypoints_planner_B.loop_ub_pd - 1 <=
16182 cartesian_waypoints_planner_B.minnanb) {
16183 cartesian_waypoints_planner_B.kstr =
16184 cartesian_waypoints_planner_B.loop_ub_pd - 1;
16185 if (tmp_3[static_cast<uint8_T>(jtype->
16186 data[cartesian_waypoints_planner_B.kstr]) & 127] != tmp_3[
16187 static_cast<int32_T>
16188 (cartesian_waypoints_planner_B.b_jo[cartesian_waypoints_planner_B.kstr])])
16189 {
16190 exitg1 = 1;
16191 } else {
16192 cartesian_waypoints_planner_B.loop_ub_pd++;
16193 }
16194 } else {
16195 cartesian_waypoints_planner_B.b_bool_is = true;
16196 exitg1 = 1;
16197 }
16198 } while (exitg1 == 0);
16199 }
16200
16201 if (cartesian_waypoints_planner_B.b_bool_is) {
16202 if (jtype->size[1] == 5) {
16203 cartesian_waypoints_planner_B.nmatched = 1;
16204 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 5;
16205 for (cartesian_waypoints_planner_B.minnanb = 0;
16206 cartesian_waypoints_planner_B.minnanb < 5;
16207 cartesian_waypoints_planner_B.minnanb++) {
16208 cartesian_waypoints_planner_B.b_m[cartesian_waypoints_planner_B.minnanb]
16209 =
16210 cartesian_waypoints_planner_B.c_vstr[cartesian_waypoints_planner_B.minnanb];
16211 }
16212 } else {
16213 if (!cartesian_waypoints_planner_B.matched) {
16214 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 5;
16215 for (cartesian_waypoints_planner_B.minnanb = 0;
16216 cartesian_waypoints_planner_B.minnanb < 5;
16217 cartesian_waypoints_planner_B.minnanb++) {
16218 cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb]
16219 =
16220 cartesian_waypoints_planner_B.c_vstr[cartesian_waypoints_planner_B.minnanb];
16221 }
16222 }
16223
16224 cartesian_waypoints_planner_B.nmatched++;
16225 guard1 = true;
16226 }
16227 } else {
16228 guard1 = true;
16229 }
16230 } else {
16231 guard1 = true;
16232 }
16233 }
16234
16235 if (guard1) {
16236 if (cartesian_waypoints_planner_B.nmatched == 0) {
16237 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 0;
16238 } else {
16239 cartesian_waypoints_planner_B.loop_ub_pd =
16240 cartesian_waypoints_planner_B.partial_match_size_idx_1 - 1;
16241 if (0 <= cartesian_waypoints_planner_B.loop_ub_pd) {
16242 memcpy(&cartesian_waypoints_planner_B.b_m[0],
16243 &cartesian_waypoints_planner_B.partial_match_data[0],
16244 (cartesian_waypoints_planner_B.loop_ub_pd + 1) * sizeof(char_T));
16245 }
16246 }
16247 }
16248
16249 if ((cartesian_waypoints_planner_B.nmatched == 0) || ((jtype->size[1] == 0) !=
16250 (cartesian_waypoints_planner_B.partial_match_size_idx_1 == 0))) {
16251 cartesian_waypoints_planner_B.partial_match_size_idx_1 = 0;
16252 } else {
16253 cartesian_waypoints_planner_B.loop_ub_pd =
16254 cartesian_waypoints_planner_B.partial_match_size_idx_1 - 1;
16255 if (0 <= cartesian_waypoints_planner_B.loop_ub_pd) {
16256 memcpy(&cartesian_waypoints_planner_B.partial_match_data[0],
16257 &cartesian_waypoints_planner_B.b_m[0],
16258 (cartesian_waypoints_planner_B.loop_ub_pd + 1) * sizeof(char_T));
16259 }
16260 }
16261
16262 cartesian_waypoints_planner_B.minnanb = obj->Type->size[0] * obj->Type->size[1];
16263 obj->Type->size[0] = 1;
16264 obj->Type->size[1] = cartesian_waypoints_planner_B.partial_match_size_idx_1;
16265 cartes_emxEnsureCapacity_char_T(obj->Type,
16266 cartesian_waypoints_planner_B.minnanb);
16267 cartesian_waypoints_planner_B.loop_ub_pd =
16268 cartesian_waypoints_planner_B.partial_match_size_idx_1 - 1;
16269 for (cartesian_waypoints_planner_B.minnanb = 0;
16270 cartesian_waypoints_planner_B.minnanb <=
16271 cartesian_waypoints_planner_B.loop_ub_pd;
16272 cartesian_waypoints_planner_B.minnanb++) {
16273 obj->Type->data[cartesian_waypoints_planner_B.minnanb] =
16274 cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb];
16275 }
16276
16277 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
16278 cartesian_waypoints_planner_B.minnanb = switch_expression->size[0] *
16279 switch_expression->size[1];
16280 switch_expression->size[0] = 1;
16281 switch_expression->size[1] = obj->Type->size[1];
16282 cartes_emxEnsureCapacity_char_T(switch_expression,
16283 cartesian_waypoints_planner_B.minnanb);
16284 cartesian_waypoints_planner_B.loop_ub_pd = obj->Type->size[0] * obj->
16285 Type->size[1] - 1;
16286 for (cartesian_waypoints_planner_B.minnanb = 0;
16287 cartesian_waypoints_planner_B.minnanb <=
16288 cartesian_waypoints_planner_B.loop_ub_pd;
16289 cartesian_waypoints_planner_B.minnanb++) {
16290 switch_expression->data[cartesian_waypoints_planner_B.minnanb] = obj->
16291 Type->data[cartesian_waypoints_planner_B.minnanb];
16292 }
16293
16294 for (cartesian_waypoints_planner_B.minnanb = 0;
16295 cartesian_waypoints_planner_B.minnanb < 8;
16296 cartesian_waypoints_planner_B.minnanb++) {
16297 cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.minnanb] =
16298 tmp_0[cartesian_waypoints_planner_B.minnanb];
16299 }
16300
16301 cartesian_waypoints_planner_B.b_bool_is = false;
16302 if (switch_expression->size[1] == 8) {
16303 cartesian_waypoints_planner_B.loop_ub_pd = 1;
16304 do {
16305 exitg1 = 0;
16306 if (cartesian_waypoints_planner_B.loop_ub_pd - 1 < 8) {
16307 cartesian_waypoints_planner_B.kstr =
16308 cartesian_waypoints_planner_B.loop_ub_pd - 1;
16309 if (switch_expression->data[cartesian_waypoints_planner_B.kstr] !=
16310 cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.kstr])
16311 {
16312 exitg1 = 1;
16313 } else {
16314 cartesian_waypoints_planner_B.loop_ub_pd++;
16315 }
16316 } else {
16317 cartesian_waypoints_planner_B.b_bool_is = true;
16318 exitg1 = 1;
16319 }
16320 } while (exitg1 == 0);
16321 }
16322
16323 if (cartesian_waypoints_planner_B.b_bool_is) {
16324 cartesian_waypoints_planner_B.minnanb = 0;
16325 } else {
16326 for (cartesian_waypoints_planner_B.minnanb = 0;
16327 cartesian_waypoints_planner_B.minnanb < 9;
16328 cartesian_waypoints_planner_B.minnanb++) {
16329 cartesian_waypoints_planner_B.b_m[cartesian_waypoints_planner_B.minnanb] =
16330 tmp_1[cartesian_waypoints_planner_B.minnanb];
16331 }
16332
16333 cartesian_waypoints_planner_B.b_bool_is = false;
16334 if (switch_expression->size[1] == 9) {
16335 cartesian_waypoints_planner_B.loop_ub_pd = 1;
16336 do {
16337 exitg1 = 0;
16338 if (cartesian_waypoints_planner_B.loop_ub_pd - 1 < 9) {
16339 cartesian_waypoints_planner_B.kstr =
16340 cartesian_waypoints_planner_B.loop_ub_pd - 1;
16341 if (switch_expression->data[cartesian_waypoints_planner_B.kstr] !=
16342 cartesian_waypoints_planner_B.b_m[cartesian_waypoints_planner_B.kstr])
16343 {
16344 exitg1 = 1;
16345 } else {
16346 cartesian_waypoints_planner_B.loop_ub_pd++;
16347 }
16348 } else {
16349 cartesian_waypoints_planner_B.b_bool_is = true;
16350 exitg1 = 1;
16351 }
16352 } while (exitg1 == 0);
16353 }
16354
16355 if (cartesian_waypoints_planner_B.b_bool_is) {
16356 cartesian_waypoints_planner_B.minnanb = 1;
16357 } else {
16358 cartesian_waypoints_planner_B.minnanb = -1;
16359 }
16360 }
16361
16362 cartesian_waypoi_emxFree_char_T(&switch_expression);
16363 switch (cartesian_waypoints_planner_B.minnanb) {
16364 case 0:
16365 cartesian_waypoints_planner_B.iv3[0] = 0;
16366 cartesian_waypoints_planner_B.iv3[1] = 0;
16367 cartesian_waypoints_planner_B.iv3[2] = 1;
16368 cartesian_waypoints_planner_B.iv3[3] = 0;
16369 cartesian_waypoints_planner_B.iv3[4] = 0;
16370 cartesian_waypoints_planner_B.iv3[5] = 0;
16371 for (cartesian_waypoints_planner_B.minnanb = 0;
16372 cartesian_waypoints_planner_B.minnanb < 6;
16373 cartesian_waypoints_planner_B.minnanb++) {
16374 cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.minnanb]
16375 =
16376 cartesian_waypoints_planner_B.iv3[cartesian_waypoints_planner_B.minnanb];
16377 }
16378
16379 cartesian_waypoints_planner_B.poslim_data_e[0] = -3.1415926535897931;
16380 cartesian_waypoints_planner_B.poslim_data_e[1] = 3.1415926535897931;
16381 obj->VelocityNumber = 1.0;
16382 obj->PositionNumber = 1.0;
16383 obj->JointAxisInternal[0] = 0.0;
16384 obj->JointAxisInternal[1] = 0.0;
16385 obj->JointAxisInternal[2] = 1.0;
16386 break;
16387
16388 case 1:
16389 cartesian_waypoints_planner_B.iv3[0] = 0;
16390 cartesian_waypoints_planner_B.iv3[1] = 0;
16391 cartesian_waypoints_planner_B.iv3[2] = 0;
16392 cartesian_waypoints_planner_B.iv3[3] = 0;
16393 cartesian_waypoints_planner_B.iv3[4] = 0;
16394 cartesian_waypoints_planner_B.iv3[5] = 1;
16395 for (cartesian_waypoints_planner_B.minnanb = 0;
16396 cartesian_waypoints_planner_B.minnanb < 6;
16397 cartesian_waypoints_planner_B.minnanb++) {
16398 cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.minnanb]
16399 =
16400 cartesian_waypoints_planner_B.iv3[cartesian_waypoints_planner_B.minnanb];
16401 }
16402
16403 cartesian_waypoints_planner_B.poslim_data_e[0] = -0.5;
16404 cartesian_waypoints_planner_B.poslim_data_e[1] = 0.5;
16405 obj->VelocityNumber = 1.0;
16406 obj->PositionNumber = 1.0;
16407 obj->JointAxisInternal[0] = 0.0;
16408 obj->JointAxisInternal[1] = 0.0;
16409 obj->JointAxisInternal[2] = 1.0;
16410 break;
16411
16412 default:
16413 for (cartesian_waypoints_planner_B.minnanb = 0;
16414 cartesian_waypoints_planner_B.minnanb < 6;
16415 cartesian_waypoints_planner_B.minnanb++) {
16416 cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.minnanb]
16417 = 0;
16418 }
16419
16420 cartesian_waypoints_planner_B.poslim_data_e[0] = 0.0;
16421 cartesian_waypoints_planner_B.poslim_data_e[1] = 0.0;
16422 obj->VelocityNumber = 0.0;
16423 obj->PositionNumber = 0.0;
16424 obj->JointAxisInternal[0] = 0.0;
16425 obj->JointAxisInternal[1] = 0.0;
16426 obj->JointAxisInternal[2] = 0.0;
16427 break;
16428 }
16429
16430 cartesian_waypoints_planner_B.minnanb = obj->MotionSubspace->size[0] *
16431 obj->MotionSubspace->size[1];
16432 obj->MotionSubspace->size[0] = 6;
16433 obj->MotionSubspace->size[1] = 1;
16434 cartes_emxEnsureCapacity_real_T(obj->MotionSubspace,
16435 cartesian_waypoints_planner_B.minnanb);
16436 for (cartesian_waypoints_planner_B.minnanb = 0;
16437 cartesian_waypoints_planner_B.minnanb < 6;
16438 cartesian_waypoints_planner_B.minnanb++) {
16439 obj->MotionSubspace->data[cartesian_waypoints_planner_B.minnanb] =
16440 cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.minnanb];
16441 }
16442
16443 cartesian_waypoints_planner_B.minnanb = obj->PositionLimitsInternal->size[0] *
16444 obj->PositionLimitsInternal->size[1];
16445 obj->PositionLimitsInternal->size[0] = 1;
16446 obj->PositionLimitsInternal->size[1] = 2;
16447 cartes_emxEnsureCapacity_real_T(obj->PositionLimitsInternal,
16448 cartesian_waypoints_planner_B.minnanb);
16449 for (cartesian_waypoints_planner_B.minnanb = 0;
16450 cartesian_waypoints_planner_B.minnanb < 2;
16451 cartesian_waypoints_planner_B.minnanb++) {
16452 obj->PositionLimitsInternal->data[cartesian_waypoints_planner_B.minnanb] =
16453 cartesian_waypoints_planner_B.poslim_data_e[cartesian_waypoints_planner_B.minnanb];
16454 }
16455
16456 cartesian_waypoints_planner_B.minnanb = obj->HomePositionInternal->size[0];
16457 obj->HomePositionInternal->size[0] = 1;
16458 cartes_emxEnsureCapacity_real_T(obj->HomePositionInternal,
16459 cartesian_waypoints_planner_B.minnanb);
16460 for (cartesian_waypoints_planner_B.minnanb = 0;
16461 cartesian_waypoints_planner_B.minnanb < 1;
16462 cartesian_waypoints_planner_B.minnanb++) {
16463 obj->HomePositionInternal->data[0] = 0.0;
16464 }
16465
16466 return b_obj;
16467}
16468
16469static w_robotics_manip_internal_Rig_T *cartesian_waypoi_RigidBody_copy(const
16470 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__e_T *iobj_0,
16471 c_rigidBodyJoint_cartesian__e_T *iobj_1, w_robotics_manip_internal_Rig_T
16472 *iobj_2)
16473{
16474 w_robotics_manip_internal_Rig_T *newbody;
16475 c_rigidBodyJoint_cartesian__e_T *newjoint;
16476 emxArray_char_T_cartesian_way_T *jtype;
16477 emxArray_char_T_cartesian_way_T *jname;
16478 emxArray_real_T_cartesian_way_T *obj_0;
16479 emxArray_real_T_cartesian_way_T *obj_1;
16480 emxArray_real_T_cartesian_way_T *obj_2;
16481 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
16482 };
16483
16484 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
16485
16486 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16487
16488 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16489
16490 int32_T exitg1;
16491 cartesian_waypoi_emxInit_char_T(&jtype, 2);
16492 cartesian_waypoints_planner_B.b_kstr_d = jtype->size[0] * jtype->size[1];
16493 jtype->size[0] = 1;
16494 jtype->size[1] = obj->NameInternal->size[1];
16495 cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_d);
16496 cartesian_waypoints_planner_B.loop_ub_kx = obj->NameInternal->size[0] *
16497 obj->NameInternal->size[1] - 1;
16498 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16499 cartesian_waypoints_planner_B.b_kstr_d <=
16500 cartesian_waypoints_planner_B.loop_ub_kx;
16501 cartesian_waypoints_planner_B.b_kstr_d++) {
16502 jtype->data[cartesian_waypoints_planner_B.b_kstr_d] = obj->
16503 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_d];
16504 }
16505
16506 newbody = iobj_2;
16507 cartesian_waypoints_planner_B.b_kstr_d = iobj_2->NameInternal->size[0] *
16508 iobj_2->NameInternal->size[1];
16509 iobj_2->NameInternal->size[0] = 1;
16510 iobj_2->NameInternal->size[1] = jtype->size[1];
16511 cartes_emxEnsureCapacity_char_T(iobj_2->NameInternal,
16512 cartesian_waypoints_planner_B.b_kstr_d);
16513 cartesian_waypoints_planner_B.loop_ub_kx = jtype->size[0] * jtype->size[1] - 1;
16514 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16515 cartesian_waypoints_planner_B.b_kstr_d <=
16516 cartesian_waypoints_planner_B.loop_ub_kx;
16517 cartesian_waypoints_planner_B.b_kstr_d++) {
16518 iobj_2->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_d] =
16519 jtype->data[cartesian_waypoints_planner_B.b_kstr_d];
16520 }
16521
16522 iobj_0->InTree = false;
16523 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16524 cartesian_waypoints_planner_B.b_kstr_d < 16;
16525 cartesian_waypoints_planner_B.b_kstr_d++) {
16526 iobj_0->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_d] =
16527 tmp[cartesian_waypoints_planner_B.b_kstr_d];
16528 }
16529
16530 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16531 cartesian_waypoints_planner_B.b_kstr_d < 16;
16532 cartesian_waypoints_planner_B.b_kstr_d++) {
16533 iobj_0->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_d] =
16534 tmp[cartesian_waypoints_planner_B.b_kstr_d];
16535 }
16536
16537 cartesian_waypoints_planner_B.b_kstr_d = iobj_0->NameInternal->size[0] *
16538 iobj_0->NameInternal->size[1];
16539 iobj_0->NameInternal->size[0] = 1;
16540 iobj_0->NameInternal->size[1] = jtype->size[1] + 4;
16541 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal,
16542 cartesian_waypoints_planner_B.b_kstr_d);
16543 cartesian_waypoints_planner_B.loop_ub_kx = jtype->size[0] * jtype->size[1];
16544 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16545 cartesian_waypoints_planner_B.b_kstr_d <
16546 cartesian_waypoints_planner_B.loop_ub_kx;
16547 cartesian_waypoints_planner_B.b_kstr_d++) {
16548 iobj_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_d] =
16549 jtype->data[cartesian_waypoints_planner_B.b_kstr_d];
16550 }
16551
16552 iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_kx] = '_';
16553 iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_kx + 1] = 'j';
16554 iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_kx + 2] = 'n';
16555 iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_kx + 3] = 't';
16556 cartesian_waypoints_planner_B.b_kstr_d = iobj_0->Type->size[0] * iobj_0->
16557 Type->size[1];
16558 iobj_0->Type->size[0] = 1;
16559 iobj_0->Type->size[1] = 5;
16560 cartes_emxEnsureCapacity_char_T(iobj_0->Type,
16561 cartesian_waypoints_planner_B.b_kstr_d);
16562 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16563 cartesian_waypoints_planner_B.b_kstr_d < 5;
16564 cartesian_waypoints_planner_B.b_kstr_d++) {
16565 iobj_0->Type->data[cartesian_waypoints_planner_B.b_kstr_d] =
16566 tmp_0[cartesian_waypoints_planner_B.b_kstr_d];
16567 }
16568
16569 cartesian_waypoints_planner_B.b_kstr_d = jtype->size[0] * jtype->size[1];
16570 jtype->size[0] = 1;
16571 jtype->size[1] = iobj_0->Type->size[1];
16572 cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_d);
16573 cartesian_waypoints_planner_B.loop_ub_kx = iobj_0->Type->size[0] *
16574 iobj_0->Type->size[1] - 1;
16575 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16576 cartesian_waypoints_planner_B.b_kstr_d <=
16577 cartesian_waypoints_planner_B.loop_ub_kx;
16578 cartesian_waypoints_planner_B.b_kstr_d++) {
16579 jtype->data[cartesian_waypoints_planner_B.b_kstr_d] = iobj_0->Type->
16580 data[cartesian_waypoints_planner_B.b_kstr_d];
16581 }
16582
16583 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16584 cartesian_waypoints_planner_B.b_kstr_d < 8;
16585 cartesian_waypoints_planner_B.b_kstr_d++) {
16586 cartesian_waypoints_planner_B.b_nr[cartesian_waypoints_planner_B.b_kstr_d] =
16587 tmp_1[cartesian_waypoints_planner_B.b_kstr_d];
16588 }
16589
16590 cartesian_waypoints_planner_B.b_bool_eb = false;
16591 if (jtype->size[1] == 8) {
16592 cartesian_waypoints_planner_B.b_kstr_d = 1;
16593 do {
16594 exitg1 = 0;
16595 if (cartesian_waypoints_planner_B.b_kstr_d - 1 < 8) {
16596 cartesian_waypoints_planner_B.loop_ub_kx =
16597 cartesian_waypoints_planner_B.b_kstr_d - 1;
16598 if (jtype->data[cartesian_waypoints_planner_B.loop_ub_kx] !=
16599 cartesian_waypoints_planner_B.b_nr[cartesian_waypoints_planner_B.loop_ub_kx])
16600 {
16601 exitg1 = 1;
16602 } else {
16603 cartesian_waypoints_planner_B.b_kstr_d++;
16604 }
16605 } else {
16606 cartesian_waypoints_planner_B.b_bool_eb = true;
16607 exitg1 = 1;
16608 }
16609 } while (exitg1 == 0);
16610 }
16611
16612 if (cartesian_waypoints_planner_B.b_bool_eb) {
16613 cartesian_waypoints_planner_B.b_kstr_d = 0;
16614 } else {
16615 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16616 cartesian_waypoints_planner_B.b_kstr_d < 9;
16617 cartesian_waypoints_planner_B.b_kstr_d++) {
16618 cartesian_waypoints_planner_B.b_i[cartesian_waypoints_planner_B.b_kstr_d] =
16619 tmp_2[cartesian_waypoints_planner_B.b_kstr_d];
16620 }
16621
16622 cartesian_waypoints_planner_B.b_bool_eb = false;
16623 if (jtype->size[1] == 9) {
16624 cartesian_waypoints_planner_B.b_kstr_d = 1;
16625 do {
16626 exitg1 = 0;
16627 if (cartesian_waypoints_planner_B.b_kstr_d - 1 < 9) {
16628 cartesian_waypoints_planner_B.loop_ub_kx =
16629 cartesian_waypoints_planner_B.b_kstr_d - 1;
16630 if (jtype->data[cartesian_waypoints_planner_B.loop_ub_kx] !=
16631 cartesian_waypoints_planner_B.b_i[cartesian_waypoints_planner_B.loop_ub_kx])
16632 {
16633 exitg1 = 1;
16634 } else {
16635 cartesian_waypoints_planner_B.b_kstr_d++;
16636 }
16637 } else {
16638 cartesian_waypoints_planner_B.b_bool_eb = true;
16639 exitg1 = 1;
16640 }
16641 } while (exitg1 == 0);
16642 }
16643
16644 if (cartesian_waypoints_planner_B.b_bool_eb) {
16645 cartesian_waypoints_planner_B.b_kstr_d = 1;
16646 } else {
16647 cartesian_waypoints_planner_B.b_kstr_d = -1;
16648 }
16649 }
16650
16651 switch (cartesian_waypoints_planner_B.b_kstr_d) {
16652 case 0:
16653 cartesian_waypoints_planner_B.iv2[0] = 0;
16654 cartesian_waypoints_planner_B.iv2[1] = 0;
16655 cartesian_waypoints_planner_B.iv2[2] = 1;
16656 cartesian_waypoints_planner_B.iv2[3] = 0;
16657 cartesian_waypoints_planner_B.iv2[4] = 0;
16658 cartesian_waypoints_planner_B.iv2[5] = 0;
16659 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16660 cartesian_waypoints_planner_B.b_kstr_d < 6;
16661 cartesian_waypoints_planner_B.b_kstr_d++) {
16662 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_d]
16663 =
16664 cartesian_waypoints_planner_B.iv2[cartesian_waypoints_planner_B.b_kstr_d];
16665 }
16666
16667 cartesian_waypoints_planner_B.poslim_data_a[0] = -3.1415926535897931;
16668 cartesian_waypoints_planner_B.poslim_data_a[1] = 3.1415926535897931;
16669 iobj_0->VelocityNumber = 1.0;
16670 iobj_0->PositionNumber = 1.0;
16671 iobj_0->JointAxisInternal[0] = 0.0;
16672 iobj_0->JointAxisInternal[1] = 0.0;
16673 iobj_0->JointAxisInternal[2] = 1.0;
16674 break;
16675
16676 case 1:
16677 cartesian_waypoints_planner_B.iv2[0] = 0;
16678 cartesian_waypoints_planner_B.iv2[1] = 0;
16679 cartesian_waypoints_planner_B.iv2[2] = 0;
16680 cartesian_waypoints_planner_B.iv2[3] = 0;
16681 cartesian_waypoints_planner_B.iv2[4] = 0;
16682 cartesian_waypoints_planner_B.iv2[5] = 1;
16683 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16684 cartesian_waypoints_planner_B.b_kstr_d < 6;
16685 cartesian_waypoints_planner_B.b_kstr_d++) {
16686 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_d]
16687 =
16688 cartesian_waypoints_planner_B.iv2[cartesian_waypoints_planner_B.b_kstr_d];
16689 }
16690
16691 cartesian_waypoints_planner_B.poslim_data_a[0] = -0.5;
16692 cartesian_waypoints_planner_B.poslim_data_a[1] = 0.5;
16693 iobj_0->VelocityNumber = 1.0;
16694 iobj_0->PositionNumber = 1.0;
16695 iobj_0->JointAxisInternal[0] = 0.0;
16696 iobj_0->JointAxisInternal[1] = 0.0;
16697 iobj_0->JointAxisInternal[2] = 1.0;
16698 break;
16699
16700 default:
16701 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16702 cartesian_waypoints_planner_B.b_kstr_d < 6;
16703 cartesian_waypoints_planner_B.b_kstr_d++) {
16704 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_d]
16705 = 0;
16706 }
16707
16708 cartesian_waypoints_planner_B.poslim_data_a[0] = 0.0;
16709 cartesian_waypoints_planner_B.poslim_data_a[1] = 0.0;
16710 iobj_0->VelocityNumber = 0.0;
16711 iobj_0->PositionNumber = 0.0;
16712 iobj_0->JointAxisInternal[0] = 0.0;
16713 iobj_0->JointAxisInternal[1] = 0.0;
16714 iobj_0->JointAxisInternal[2] = 0.0;
16715 break;
16716 }
16717
16718 cartesian_waypoints_planner_B.b_kstr_d = iobj_0->MotionSubspace->size[0] *
16719 iobj_0->MotionSubspace->size[1];
16720 iobj_0->MotionSubspace->size[0] = 6;
16721 iobj_0->MotionSubspace->size[1] = 1;
16722 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace,
16723 cartesian_waypoints_planner_B.b_kstr_d);
16724 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16725 cartesian_waypoints_planner_B.b_kstr_d < 6;
16726 cartesian_waypoints_planner_B.b_kstr_d++) {
16727 iobj_0->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_d] =
16728 cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_d];
16729 }
16730
16731 cartesian_waypoints_planner_B.b_kstr_d = iobj_0->PositionLimitsInternal->size
16732 [0] * iobj_0->PositionLimitsInternal->size[1];
16733 iobj_0->PositionLimitsInternal->size[0] = 1;
16734 iobj_0->PositionLimitsInternal->size[1] = 2;
16735 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal,
16736 cartesian_waypoints_planner_B.b_kstr_d);
16737 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16738 cartesian_waypoints_planner_B.b_kstr_d < 2;
16739 cartesian_waypoints_planner_B.b_kstr_d++) {
16740 iobj_0->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_d]
16741 =
16742 cartesian_waypoints_planner_B.poslim_data_a[cartesian_waypoints_planner_B.b_kstr_d];
16743 }
16744
16745 cartesian_waypoints_planner_B.b_kstr_d = iobj_0->HomePositionInternal->size[0];
16746 iobj_0->HomePositionInternal->size[0] = 1;
16747 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal,
16748 cartesian_waypoints_planner_B.b_kstr_d);
16749 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16750 cartesian_waypoints_planner_B.b_kstr_d < 1;
16751 cartesian_waypoints_planner_B.b_kstr_d++) {
16752 iobj_0->HomePositionInternal->data[0] = 0.0;
16753 }
16754
16755 iobj_2->JointInternal = iobj_0;
16756 iobj_2->Index = -1.0;
16757 iobj_2->ParentIndex = -1.0;
16758 cartesian_waypoints_planner_B.b_kstr_d = jtype->size[0] * jtype->size[1];
16759 jtype->size[0] = 1;
16760 jtype->size[1] = obj->JointInternal.Type->size[1];
16761 cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_d);
16762 cartesian_waypoints_planner_B.loop_ub_kx = obj->JointInternal.Type->size[0] *
16763 obj->JointInternal.Type->size[1] - 1;
16764 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16765 cartesian_waypoints_planner_B.b_kstr_d <=
16766 cartesian_waypoints_planner_B.loop_ub_kx;
16767 cartesian_waypoints_planner_B.b_kstr_d++) {
16768 jtype->data[cartesian_waypoints_planner_B.b_kstr_d] =
16769 obj->JointInternal.Type->data[cartesian_waypoints_planner_B.b_kstr_d];
16770 }
16771
16772 cartesian_waypoi_emxInit_char_T(&jname, 2);
16773 cartesian_waypoints_planner_B.b_kstr_d = jname->size[0] * jname->size[1];
16774 jname->size[0] = 1;
16775 jname->size[1] = obj->JointInternal.NameInternal->size[1];
16776 cartes_emxEnsureCapacity_char_T(jname, cartesian_waypoints_planner_B.b_kstr_d);
16777 cartesian_waypoints_planner_B.loop_ub_kx = obj->
16778 JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size
16779 [1] - 1;
16780 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16781 cartesian_waypoints_planner_B.b_kstr_d <=
16782 cartesian_waypoints_planner_B.loop_ub_kx;
16783 cartesian_waypoints_planner_B.b_kstr_d++) {
16784 jname->data[cartesian_waypoints_planner_B.b_kstr_d] =
16785 obj->JointInternal.NameInternal->
16786 data[cartesian_waypoints_planner_B.b_kstr_d];
16787 }
16788
16789 newjoint = c_rigidBodyJoint_rigidBodyJoint(iobj_1, jname, jtype);
16790 cartesian_waypoints_planner_B.b_kstr_d = jtype->size[0] * jtype->size[1];
16791 jtype->size[0] = 1;
16792 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
16793 cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_d);
16794 cartesian_waypoints_planner_B.loop_ub_kx = obj->
16795 JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size
16796 [1] - 1;
16797 cartesian_waypoi_emxFree_char_T(&jname);
16798 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16799 cartesian_waypoints_planner_B.b_kstr_d <=
16800 cartesian_waypoints_planner_B.loop_ub_kx;
16801 cartesian_waypoints_planner_B.b_kstr_d++) {
16802 jtype->data[cartesian_waypoints_planner_B.b_kstr_d] =
16803 obj->JointInternal.NameInternal->
16804 data[cartesian_waypoints_planner_B.b_kstr_d];
16805 }
16806
16807 if (jtype->size[1] != 0) {
16808 cartesian_waypoints_planner_B.b_kstr_d = jtype->size[0] * jtype->size[1];
16809 jtype->size[0] = 1;
16810 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
16811 cartes_emxEnsureCapacity_char_T(jtype,
16812 cartesian_waypoints_planner_B.b_kstr_d);
16813 cartesian_waypoints_planner_B.loop_ub_kx = obj->
16814 JointInternal.NameInternal->size[0] * obj->
16815 JointInternal.NameInternal->size[1] - 1;
16816 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16817 cartesian_waypoints_planner_B.b_kstr_d <=
16818 cartesian_waypoints_planner_B.loop_ub_kx;
16819 cartesian_waypoints_planner_B.b_kstr_d++) {
16820 jtype->data[cartesian_waypoints_planner_B.b_kstr_d] =
16821 obj->JointInternal.NameInternal->
16822 data[cartesian_waypoints_planner_B.b_kstr_d];
16823 }
16824
16825 if (!newjoint->InTree) {
16826 cartesian_waypoints_planner_B.b_kstr_d = newjoint->NameInternal->size[0] *
16827 newjoint->NameInternal->size[1];
16828 newjoint->NameInternal->size[0] = 1;
16829 newjoint->NameInternal->size[1] = jtype->size[1];
16830 cartes_emxEnsureCapacity_char_T(newjoint->NameInternal,
16831 cartesian_waypoints_planner_B.b_kstr_d);
16832 cartesian_waypoints_planner_B.loop_ub_kx = jtype->size[0] * jtype->size[1]
16833 - 1;
16834 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16835 cartesian_waypoints_planner_B.b_kstr_d <=
16836 cartesian_waypoints_planner_B.loop_ub_kx;
16837 cartesian_waypoints_planner_B.b_kstr_d++) {
16838 newjoint->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_d] =
16839 jtype->data[cartesian_waypoints_planner_B.b_kstr_d];
16840 }
16841 }
16842 }
16843
16844 cartesian_waypoi_emxFree_char_T(&jtype);
16845 cartesian_waypoi_emxInit_real_T(&obj_0, 1);
16846 cartesian_waypoints_planner_B.loop_ub_kx =
16847 obj->JointInternal.PositionLimitsInternal->size[0] *
16848 obj->JointInternal.PositionLimitsInternal->size[1];
16849 cartesian_waypoints_planner_B.b_kstr_d = newjoint->
16850 PositionLimitsInternal->size[0] * newjoint->PositionLimitsInternal->size[1];
16851 newjoint->PositionLimitsInternal->size[0] =
16852 obj->JointInternal.PositionLimitsInternal->size[0];
16853 newjoint->PositionLimitsInternal->size[1] = 2;
16854 cartes_emxEnsureCapacity_real_T(newjoint->PositionLimitsInternal,
16855 cartesian_waypoints_planner_B.b_kstr_d);
16856 cartesian_waypoints_planner_B.b_kstr_d = obj_0->size[0];
16857 obj_0->size[0] = cartesian_waypoints_planner_B.loop_ub_kx;
16858 cartes_emxEnsureCapacity_real_T(obj_0, cartesian_waypoints_planner_B.b_kstr_d);
16859 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16860 cartesian_waypoints_planner_B.b_kstr_d <
16861 cartesian_waypoints_planner_B.loop_ub_kx;
16862 cartesian_waypoints_planner_B.b_kstr_d++) {
16863 obj_0->data[cartesian_waypoints_planner_B.b_kstr_d] =
16864 obj->JointInternal.PositionLimitsInternal->
16865 data[cartesian_waypoints_planner_B.b_kstr_d];
16866 }
16867
16868 cartesian_waypoints_planner_B.loop_ub_kx = obj_0->size[0];
16869 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16870 cartesian_waypoints_planner_B.b_kstr_d <
16871 cartesian_waypoints_planner_B.loop_ub_kx;
16872 cartesian_waypoints_planner_B.b_kstr_d++) {
16873 newjoint->PositionLimitsInternal->
16874 data[cartesian_waypoints_planner_B.b_kstr_d] = obj_0->
16875 data[cartesian_waypoints_planner_B.b_kstr_d];
16876 }
16877
16878 cartesian_waypoi_emxFree_real_T(&obj_0);
16879 cartesian_waypoi_emxInit_real_T(&obj_1, 1);
16880 cartesian_waypoints_planner_B.b_kstr_d = obj_1->size[0];
16881 obj_1->size[0] = obj->JointInternal.HomePositionInternal->size[0];
16882 cartes_emxEnsureCapacity_real_T(obj_1, cartesian_waypoints_planner_B.b_kstr_d);
16883 cartesian_waypoints_planner_B.loop_ub_kx =
16884 obj->JointInternal.HomePositionInternal->size[0];
16885 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16886 cartesian_waypoints_planner_B.b_kstr_d <
16887 cartesian_waypoints_planner_B.loop_ub_kx;
16888 cartesian_waypoints_planner_B.b_kstr_d++) {
16889 obj_1->data[cartesian_waypoints_planner_B.b_kstr_d] =
16890 obj->JointInternal.HomePositionInternal->
16891 data[cartesian_waypoints_planner_B.b_kstr_d];
16892 }
16893
16894 cartesian_waypoints_planner_B.b_kstr_d = newjoint->HomePositionInternal->size
16895 [0];
16896 newjoint->HomePositionInternal->size[0] = obj_1->size[0];
16897 cartes_emxEnsureCapacity_real_T(newjoint->HomePositionInternal,
16898 cartesian_waypoints_planner_B.b_kstr_d);
16899 cartesian_waypoints_planner_B.loop_ub_kx = obj_1->size[0];
16900 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16901 cartesian_waypoints_planner_B.b_kstr_d <
16902 cartesian_waypoints_planner_B.loop_ub_kx;
16903 cartesian_waypoints_planner_B.b_kstr_d++) {
16904 newjoint->HomePositionInternal->data[cartesian_waypoints_planner_B.b_kstr_d]
16905 = obj_1->data[cartesian_waypoints_planner_B.b_kstr_d];
16906 }
16907
16908 cartesian_waypoi_emxFree_real_T(&obj_1);
16909 cartesian_waypoints_planner_B.obj_idx_0 = obj->
16910 JointInternal.JointAxisInternal[0];
16911 cartesian_waypoints_planner_B.obj_idx_1 = obj->
16912 JointInternal.JointAxisInternal[1];
16913 cartesian_waypoints_planner_B.obj_idx_2 = obj->
16914 JointInternal.JointAxisInternal[2];
16915 newjoint->JointAxisInternal[0] = cartesian_waypoints_planner_B.obj_idx_0;
16916 newjoint->JointAxisInternal[1] = cartesian_waypoints_planner_B.obj_idx_1;
16917 newjoint->JointAxisInternal[2] = cartesian_waypoints_planner_B.obj_idx_2;
16918 cartesian_waypoi_emxInit_real_T(&obj_2, 1);
16919 cartesian_waypoints_planner_B.loop_ub_kx = obj->
16920 JointInternal.MotionSubspace->size[0] * obj->
16921 JointInternal.MotionSubspace->size[1];
16922 cartesian_waypoints_planner_B.b_kstr_d = newjoint->MotionSubspace->size[0] *
16923 newjoint->MotionSubspace->size[1];
16924 newjoint->MotionSubspace->size[0] = 6;
16925 newjoint->MotionSubspace->size[1] = obj->JointInternal.MotionSubspace->size[1];
16926 cartes_emxEnsureCapacity_real_T(newjoint->MotionSubspace,
16927 cartesian_waypoints_planner_B.b_kstr_d);
16928 cartesian_waypoints_planner_B.b_kstr_d = obj_2->size[0];
16929 obj_2->size[0] = cartesian_waypoints_planner_B.loop_ub_kx;
16930 cartes_emxEnsureCapacity_real_T(obj_2, cartesian_waypoints_planner_B.b_kstr_d);
16931 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16932 cartesian_waypoints_planner_B.b_kstr_d <
16933 cartesian_waypoints_planner_B.loop_ub_kx;
16934 cartesian_waypoints_planner_B.b_kstr_d++) {
16935 obj_2->data[cartesian_waypoints_planner_B.b_kstr_d] =
16936 obj->JointInternal.MotionSubspace->
16937 data[cartesian_waypoints_planner_B.b_kstr_d];
16938 }
16939
16940 cartesian_waypoints_planner_B.loop_ub_kx = obj_2->size[0];
16941 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16942 cartesian_waypoints_planner_B.b_kstr_d <
16943 cartesian_waypoints_planner_B.loop_ub_kx;
16944 cartesian_waypoints_planner_B.b_kstr_d++) {
16945 newjoint->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_d] =
16946 obj_2->data[cartesian_waypoints_planner_B.b_kstr_d];
16947 }
16948
16949 cartesian_waypoi_emxFree_real_T(&obj_2);
16950 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16951 cartesian_waypoints_planner_B.b_kstr_d < 16;
16952 cartesian_waypoints_planner_B.b_kstr_d++) {
16953 cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_d] =
16954 obj->
16955 JointInternal.JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_d];
16956 }
16957
16958 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16959 cartesian_waypoints_planner_B.b_kstr_d < 16;
16960 cartesian_waypoints_planner_B.b_kstr_d++) {
16961 newjoint->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_d] =
16962 cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_d];
16963 }
16964
16965 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16966 cartesian_waypoints_planner_B.b_kstr_d < 16;
16967 cartesian_waypoints_planner_B.b_kstr_d++) {
16968 cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_d] =
16969 obj->
16970 JointInternal.ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_d];
16971 }
16972
16973 for (cartesian_waypoints_planner_B.b_kstr_d = 0;
16974 cartesian_waypoints_planner_B.b_kstr_d < 16;
16975 cartesian_waypoints_planner_B.b_kstr_d++) {
16976 newjoint->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_d] =
16977 cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_d];
16978 }
16979
16980 iobj_2->JointInternal = newjoint;
16981 return newbody;
16982}
16983
16984static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
16985 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_way_T
16986 *parentName, c_rigidBodyJoint_cartesian__e_T *iobj_0,
16987 c_rigidBodyJoint_cartesian__e_T *iobj_1, w_robotics_manip_internal_Rig_T
16988 *iobj_2)
16989{
16990 w_robotics_manip_internal_Rig_T *body;
16991 c_rigidBodyJoint_cartesian__e_T *jnt;
16992 emxArray_char_T_cartesian_way_T *bname;
16993 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
16994
16995 boolean_T exitg1;
16996 int32_T exitg2;
16997 cartesian_waypoi_emxInit_char_T(&bname, 2);
16998 cartesian_waypoints_planner_B.pid = -1.0;
16999 cartesian_waypoints_planner_B.b_kstr_c = bname->size[0] * bname->size[1];
17000 bname->size[0] = 1;
17001 bname->size[1] = obj->Base.NameInternal->size[1];
17002 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr_c);
17003 cartesian_waypoints_planner_B.loop_ub_k = obj->Base.NameInternal->size[0] *
17004 obj->Base.NameInternal->size[1] - 1;
17005 for (cartesian_waypoints_planner_B.b_kstr_c = 0;
17006 cartesian_waypoints_planner_B.b_kstr_c <=
17007 cartesian_waypoints_planner_B.loop_ub_k;
17008 cartesian_waypoints_planner_B.b_kstr_c++) {
17009 bname->data[cartesian_waypoints_planner_B.b_kstr_c] = obj->
17010 Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_c];
17011 }
17012
17013 if (cartesian_waypoints_plan_strcmp(bname, parentName)) {
17014 cartesian_waypoints_planner_B.pid = 0.0;
17015 } else {
17016 cartesian_waypoints_planner_B.b_index_f = obj->NumBodies;
17017 cartesian_waypoints_planner_B.b_i_gr = 0;
17018 exitg1 = false;
17019 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_gr <=
17020 static_cast<int32_T>
17021 (cartesian_waypoints_planner_B.b_index_f) - 1)) {
17022 body = obj->Bodies[cartesian_waypoints_planner_B.b_i_gr];
17023 cartesian_waypoints_planner_B.b_kstr_c = bname->size[0] * bname->size[1];
17024 bname->size[0] = 1;
17025 bname->size[1] = body->NameInternal->size[1];
17026 cartes_emxEnsureCapacity_char_T(bname,
17027 cartesian_waypoints_planner_B.b_kstr_c);
17028 cartesian_waypoints_planner_B.loop_ub_k = body->NameInternal->size[0] *
17029 body->NameInternal->size[1] - 1;
17030 for (cartesian_waypoints_planner_B.b_kstr_c = 0;
17031 cartesian_waypoints_planner_B.b_kstr_c <=
17032 cartesian_waypoints_planner_B.loop_ub_k;
17033 cartesian_waypoints_planner_B.b_kstr_c++) {
17034 bname->data[cartesian_waypoints_planner_B.b_kstr_c] = body->
17035 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_c];
17036 }
17037
17038 if (cartesian_waypoints_plan_strcmp(bname, parentName)) {
17039 cartesian_waypoints_planner_B.pid = static_cast<real_T>
17040 (cartesian_waypoints_planner_B.b_i_gr) + 1.0;
17041 exitg1 = true;
17042 } else {
17043 cartesian_waypoints_planner_B.b_i_gr++;
17044 }
17045 }
17046 }
17047
17048 cartesian_waypoints_planner_B.b_index_f = obj->NumBodies + 1.0;
17049 body = cartesian_waypoi_RigidBody_copy(bodyin, iobj_1, iobj_0, iobj_2);
17050 obj->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.b_index_f) - 1]
17051 = body;
17052 body->Index = cartesian_waypoints_planner_B.b_index_f;
17053 body->ParentIndex = cartesian_waypoints_planner_B.pid;
17054 body->JointInternal->InTree = true;
17055 obj->NumBodies++;
17056 jnt = body->JointInternal;
17057 cartesian_waypoints_planner_B.b_kstr_c = bname->size[0] * bname->size[1];
17058 bname->size[0] = 1;
17059 bname->size[1] = jnt->Type->size[1];
17060 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr_c);
17061 cartesian_waypoints_planner_B.loop_ub_k = jnt->Type->size[0] * jnt->Type->
17062 size[1] - 1;
17063 for (cartesian_waypoints_planner_B.b_kstr_c = 0;
17064 cartesian_waypoints_planner_B.b_kstr_c <=
17065 cartesian_waypoints_planner_B.loop_ub_k;
17066 cartesian_waypoints_planner_B.b_kstr_c++) {
17067 bname->data[cartesian_waypoints_planner_B.b_kstr_c] = jnt->Type->
17068 data[cartesian_waypoints_planner_B.b_kstr_c];
17069 }
17070
17071 for (cartesian_waypoints_planner_B.b_kstr_c = 0;
17072 cartesian_waypoints_planner_B.b_kstr_c < 5;
17073 cartesian_waypoints_planner_B.b_kstr_c++) {
17074 cartesian_waypoints_planner_B.b_dv[cartesian_waypoints_planner_B.b_kstr_c] =
17075 tmp[cartesian_waypoints_planner_B.b_kstr_c];
17076 }
17077
17078 cartesian_waypoints_planner_B.b_bool_i = false;
17079 if (bname->size[1] == 5) {
17080 cartesian_waypoints_planner_B.b_kstr_c = 1;
17081 do {
17082 exitg2 = 0;
17083 if (cartesian_waypoints_planner_B.b_kstr_c - 1 < 5) {
17084 cartesian_waypoints_planner_B.loop_ub_k =
17085 cartesian_waypoints_planner_B.b_kstr_c - 1;
17086 if (bname->data[cartesian_waypoints_planner_B.loop_ub_k] !=
17087 cartesian_waypoints_planner_B.b_dv[cartesian_waypoints_planner_B.loop_ub_k])
17088 {
17089 exitg2 = 1;
17090 } else {
17091 cartesian_waypoints_planner_B.b_kstr_c++;
17092 }
17093 } else {
17094 cartesian_waypoints_planner_B.b_bool_i = true;
17095 exitg2 = 1;
17096 }
17097 } while (exitg2 == 0);
17098 }
17099
17100 cartesian_waypoi_emxFree_char_T(&bname);
17101 if (!cartesian_waypoints_planner_B.b_bool_i) {
17102 obj->NumNonFixedBodies++;
17103 jnt = body->JointInternal;
17104 cartesian_waypoints_planner_B.b_kstr_c = static_cast<int32_T>(body->Index) -
17105 1;
17106 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_c] =
17107 obj->PositionNumber + 1.0;
17108 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_c + 8] =
17109 obj->PositionNumber + jnt->PositionNumber;
17110 jnt = body->JointInternal;
17111 cartesian_waypoints_planner_B.b_kstr_c = static_cast<int32_T>(body->Index) -
17112 1;
17113 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_c] =
17114 obj->VelocityNumber + 1.0;
17115 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_c + 8] =
17116 obj->VelocityNumber + jnt->VelocityNumber;
17117 } else {
17118 cartesian_waypoints_planner_B.b_kstr_c = static_cast<int32_T>(body->Index);
17119 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_c - 1] = 0.0;
17120 obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_c + 7] = -1.0;
17121 cartesian_waypoints_planner_B.b_kstr_c = static_cast<int32_T>(body->Index);
17122 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_c - 1] = 0.0;
17123 obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_c + 7] = -1.0;
17124 }
17125
17126 jnt = body->JointInternal;
17127 obj->PositionNumber += jnt->PositionNumber;
17128 jnt = body->JointInternal;
17129 obj->VelocityNumber += jnt->VelocityNumber;
17130}
17131
17132static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
17133 y_robotics_manip_internal_Rig_T *rigidbodytree,
17134 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
17135 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
17136 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
17137 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
17138 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
17139 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
17140 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
17141 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
17142 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
17143 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
17144 c_rigidBodyJoint_cartesian__e_T *iobj_15, c_rigidBodyJoint_cartesian__e_T
17145 *iobj_16, c_rigidBodyJoint_cartesian__e_T *iobj_17,
17146 c_rigidBodyJoint_cartesian__e_T *iobj_18, c_rigidBodyJoint_cartesian__e_T
17147 *iobj_19, c_rigidBodyJoint_cartesian__e_T *iobj_20,
17148 c_rigidBodyJoint_cartesian__e_T *iobj_21, c_rigidBodyJoint_cartesian__e_T
17149 *iobj_22, c_rigidBodyJoint_cartesian__e_T *iobj_23,
17150 c_rigidBodyJoint_cartesian__e_T *iobj_24, c_rigidBodyJoint_cartesian__e_T
17151 *iobj_25, c_rigidBodyJoint_cartesian__e_T *iobj_26,
17152 c_rigidBodyJoint_cartesian__e_T *iobj_27, c_rigidBodyJoint_cartesian__e_T
17153 *iobj_28, c_rigidBodyJoint_cartesian__e_T *iobj_29,
17154 c_rigidBodyJoint_cartesian__e_T *iobj_30, c_rigidBodyJoint_cartesian__e_T
17155 *iobj_31, c_rigidBodyJoint_cartesian__e_T *iobj_32,
17156 c_rigidBodyJoint_cartesian__e_T *iobj_33, c_rigidBodyJoint_cartesian__e_T
17157 *iobj_34, c_rigidBodyJoint_cartesian__e_T *iobj_35,
17158 c_rigidBodyJoint_cartesian__e_T *iobj_36, c_rigidBodyJoint_cartesian__e_T
17159 *iobj_37, c_rigidBodyJoint_cartesian__e_T *iobj_38,
17160 c_rigidBodyJoint_cartesian__e_T *iobj_39, w_robotics_manip_internal_Rig_T
17161 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41)
17162{
17163 x_robotics_manip_internal_Rig_T *newrobot;
17164 v_robotics_manip_internal_Rig_T *body;
17165 v_robotics_manip_internal_Rig_T *parent;
17166 emxArray_char_T_cartesian_way_T *b_basename;
17167 w_robotics_manip_internal_Rig_T *body_0;
17168 c_rigidBodyJoint_cartesian__e_T *jnt;
17169 emxArray_char_T_cartesian_way_T *bname;
17170 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
17171
17172 boolean_T exitg1;
17173 int32_T exitg2;
17174 cartesian_waypoi_emxInit_char_T(&b_basename, 2);
17175 newrobot = RigidBodyTree_RigidBodyTree_ev(iobj_41, iobj_0, iobj_1, iobj_2,
17176 iobj_3, iobj_4, iobj_5, iobj_6, iobj_15, iobj_16, iobj_17, iobj_18, iobj_19,
17177 iobj_20, iobj_21, iobj_22, iobj_39, iobj_40);
17178 cartesian_waypoints_planner_B.b_kstr_h = b_basename->size[0] *
17179 b_basename->size[1];
17180 b_basename->size[0] = 1;
17181 b_basename->size[1] = rigidbodytree->Base.NameInternal->size[1];
17182 cartes_emxEnsureCapacity_char_T(b_basename,
17183 cartesian_waypoints_planner_B.b_kstr_h);
17184 cartesian_waypoints_planner_B.loop_ub_m = rigidbodytree->
17185 Base.NameInternal->size[0] * rigidbodytree->Base.NameInternal->size[1] - 1;
17186 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17187 cartesian_waypoints_planner_B.b_kstr_h <=
17188 cartesian_waypoints_planner_B.loop_ub_m;
17189 cartesian_waypoints_planner_B.b_kstr_h++) {
17190 b_basename->data[cartesian_waypoints_planner_B.b_kstr_h] =
17191 rigidbodytree->Base.NameInternal->
17192 data[cartesian_waypoints_planner_B.b_kstr_h];
17193 }
17194
17195 cartesian_waypoi_emxInit_char_T(&bname, 2);
17196 cartesian_waypoints_planner_B.bid_e = -1.0;
17197 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17198 bname->size[0] = 1;
17199 bname->size[1] = newrobot->Base.NameInternal->size[1];
17200 cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr_h);
17201 cartesian_waypoints_planner_B.loop_ub_m = newrobot->Base.NameInternal->size[0]
17202 * newrobot->Base.NameInternal->size[1] - 1;
17203 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17204 cartesian_waypoints_planner_B.b_kstr_h <=
17205 cartesian_waypoints_planner_B.loop_ub_m;
17206 cartesian_waypoints_planner_B.b_kstr_h++) {
17207 bname->data[cartesian_waypoints_planner_B.b_kstr_h] =
17208 newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17209 }
17210
17211 if (cartesian_waypoints_plan_strcmp(bname, b_basename)) {
17212 cartesian_waypoints_planner_B.bid_e = 0.0;
17213 } else {
17214 cartesian_waypoints_planner_B.b_index = newrobot->NumBodies;
17215 cartesian_waypoints_planner_B.b_i_i = 0;
17216 exitg1 = false;
17217 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_i <=
17218 static_cast<int32_T>
17219 (cartesian_waypoints_planner_B.b_index) - 1)) {
17220 body_0 = newrobot->Bodies[cartesian_waypoints_planner_B.b_i_i];
17221 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17222 bname->size[0] = 1;
17223 bname->size[1] = body_0->NameInternal->size[1];
17224 cartes_emxEnsureCapacity_char_T(bname,
17225 cartesian_waypoints_planner_B.b_kstr_h);
17226 cartesian_waypoints_planner_B.loop_ub_m = body_0->NameInternal->size[0] *
17227 body_0->NameInternal->size[1] - 1;
17228 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17229 cartesian_waypoints_planner_B.b_kstr_h <=
17230 cartesian_waypoints_planner_B.loop_ub_m;
17231 cartesian_waypoints_planner_B.b_kstr_h++) {
17232 bname->data[cartesian_waypoints_planner_B.b_kstr_h] =
17233 body_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17234 }
17235
17236 if (cartesian_waypoints_plan_strcmp(bname, b_basename)) {
17237 cartesian_waypoints_planner_B.bid_e = static_cast<real_T>
17238 (cartesian_waypoints_planner_B.b_i_i) + 1.0;
17239 exitg1 = true;
17240 } else {
17241 cartesian_waypoints_planner_B.b_i_i++;
17242 }
17243 }
17244 }
17245
17246 if ((!(cartesian_waypoints_planner_B.bid_e == 0.0)) &&
17247 (cartesian_waypoints_planner_B.bid_e < 0.0)) {
17248 cartesian_waypoints_planner_B.b_kstr_h = newrobot->Base.NameInternal->size[0]
17249 * newrobot->Base.NameInternal->size[1];
17250 newrobot->Base.NameInternal->size[0] = 1;
17251 newrobot->Base.NameInternal->size[1] = b_basename->size[1];
17252 cartes_emxEnsureCapacity_char_T(newrobot->Base.NameInternal,
17253 cartesian_waypoints_planner_B.b_kstr_h);
17254 cartesian_waypoints_planner_B.loop_ub_m = b_basename->size[0] *
17255 b_basename->size[1] - 1;
17256 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17257 cartesian_waypoints_planner_B.b_kstr_h <=
17258 cartesian_waypoints_planner_B.loop_ub_m;
17259 cartesian_waypoints_planner_B.b_kstr_h++) {
17260 newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h] =
17261 b_basename->data[cartesian_waypoints_planner_B.b_kstr_h];
17262 }
17263 }
17264
17265 if (1.0 <= rigidbodytree->NumBodies) {
17266 body = rigidbodytree->Bodies[0];
17267 cartesian_waypoints_planner_B.bid_e = body->ParentIndex;
17268 if (cartesian_waypoints_planner_B.bid_e > 0.0) {
17269 parent = rigidbodytree->Bodies[static_cast<int32_T>
17270 (cartesian_waypoints_planner_B.bid_e) - 1];
17271 } else {
17272 parent = &rigidbodytree->Base;
17273 }
17274
17275 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17276 bname->size[0] = 1;
17277 bname->size[1] = parent->NameInternal->size[1];
17278 cartes_emxEnsureCapacity_char_T(bname,
17279 cartesian_waypoints_planner_B.b_kstr_h);
17280 cartesian_waypoints_planner_B.loop_ub_m = parent->NameInternal->size[0] *
17281 parent->NameInternal->size[1] - 1;
17282 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17283 cartesian_waypoints_planner_B.b_kstr_h <=
17284 cartesian_waypoints_planner_B.loop_ub_m;
17285 cartesian_waypoints_planner_B.b_kstr_h++) {
17286 bname->data[cartesian_waypoints_planner_B.b_kstr_h] = parent->
17287 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17288 }
17289
17290 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_24, iobj_23,
17291 iobj_7);
17292 }
17293
17294 if (2.0 <= rigidbodytree->NumBodies) {
17295 body = rigidbodytree->Bodies[1];
17296 cartesian_waypoints_planner_B.bid_e = body->ParentIndex;
17297 if (cartesian_waypoints_planner_B.bid_e > 0.0) {
17298 parent = rigidbodytree->Bodies[static_cast<int32_T>
17299 (cartesian_waypoints_planner_B.bid_e) - 1];
17300 } else {
17301 parent = &rigidbodytree->Base;
17302 }
17303
17304 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17305 bname->size[0] = 1;
17306 bname->size[1] = parent->NameInternal->size[1];
17307 cartes_emxEnsureCapacity_char_T(bname,
17308 cartesian_waypoints_planner_B.b_kstr_h);
17309 cartesian_waypoints_planner_B.loop_ub_m = parent->NameInternal->size[0] *
17310 parent->NameInternal->size[1] - 1;
17311 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17312 cartesian_waypoints_planner_B.b_kstr_h <=
17313 cartesian_waypoints_planner_B.loop_ub_m;
17314 cartesian_waypoints_planner_B.b_kstr_h++) {
17315 bname->data[cartesian_waypoints_planner_B.b_kstr_h] = parent->
17316 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17317 }
17318
17319 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_26, iobj_25,
17320 iobj_8);
17321 }
17322
17323 if (3.0 <= rigidbodytree->NumBodies) {
17324 body = rigidbodytree->Bodies[2];
17325 cartesian_waypoints_planner_B.bid_e = body->ParentIndex;
17326 if (cartesian_waypoints_planner_B.bid_e > 0.0) {
17327 parent = rigidbodytree->Bodies[static_cast<int32_T>
17328 (cartesian_waypoints_planner_B.bid_e) - 1];
17329 } else {
17330 parent = &rigidbodytree->Base;
17331 }
17332
17333 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17334 bname->size[0] = 1;
17335 bname->size[1] = parent->NameInternal->size[1];
17336 cartes_emxEnsureCapacity_char_T(bname,
17337 cartesian_waypoints_planner_B.b_kstr_h);
17338 cartesian_waypoints_planner_B.loop_ub_m = parent->NameInternal->size[0] *
17339 parent->NameInternal->size[1] - 1;
17340 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17341 cartesian_waypoints_planner_B.b_kstr_h <=
17342 cartesian_waypoints_planner_B.loop_ub_m;
17343 cartesian_waypoints_planner_B.b_kstr_h++) {
17344 bname->data[cartesian_waypoints_planner_B.b_kstr_h] = parent->
17345 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17346 }
17347
17348 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_28, iobj_27,
17349 iobj_9);
17350 }
17351
17352 if (4.0 <= rigidbodytree->NumBodies) {
17353 body = rigidbodytree->Bodies[3];
17354 cartesian_waypoints_planner_B.bid_e = body->ParentIndex;
17355 if (cartesian_waypoints_planner_B.bid_e > 0.0) {
17356 parent = rigidbodytree->Bodies[static_cast<int32_T>
17357 (cartesian_waypoints_planner_B.bid_e) - 1];
17358 } else {
17359 parent = &rigidbodytree->Base;
17360 }
17361
17362 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17363 bname->size[0] = 1;
17364 bname->size[1] = parent->NameInternal->size[1];
17365 cartes_emxEnsureCapacity_char_T(bname,
17366 cartesian_waypoints_planner_B.b_kstr_h);
17367 cartesian_waypoints_planner_B.loop_ub_m = parent->NameInternal->size[0] *
17368 parent->NameInternal->size[1] - 1;
17369 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17370 cartesian_waypoints_planner_B.b_kstr_h <=
17371 cartesian_waypoints_planner_B.loop_ub_m;
17372 cartesian_waypoints_planner_B.b_kstr_h++) {
17373 bname->data[cartesian_waypoints_planner_B.b_kstr_h] = parent->
17374 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17375 }
17376
17377 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_30, iobj_29,
17378 iobj_10);
17379 }
17380
17381 if (5.0 <= rigidbodytree->NumBodies) {
17382 body = rigidbodytree->Bodies[4];
17383 cartesian_waypoints_planner_B.bid_e = body->ParentIndex;
17384 if (cartesian_waypoints_planner_B.bid_e > 0.0) {
17385 parent = rigidbodytree->Bodies[static_cast<int32_T>
17386 (cartesian_waypoints_planner_B.bid_e) - 1];
17387 } else {
17388 parent = &rigidbodytree->Base;
17389 }
17390
17391 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17392 bname->size[0] = 1;
17393 bname->size[1] = parent->NameInternal->size[1];
17394 cartes_emxEnsureCapacity_char_T(bname,
17395 cartesian_waypoints_planner_B.b_kstr_h);
17396 cartesian_waypoints_planner_B.loop_ub_m = parent->NameInternal->size[0] *
17397 parent->NameInternal->size[1] - 1;
17398 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17399 cartesian_waypoints_planner_B.b_kstr_h <=
17400 cartesian_waypoints_planner_B.loop_ub_m;
17401 cartesian_waypoints_planner_B.b_kstr_h++) {
17402 bname->data[cartesian_waypoints_planner_B.b_kstr_h] = parent->
17403 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17404 }
17405
17406 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_32, iobj_31,
17407 iobj_11);
17408 }
17409
17410 if (6.0 <= rigidbodytree->NumBodies) {
17411 body = rigidbodytree->Bodies[5];
17412 cartesian_waypoints_planner_B.bid_e = body->ParentIndex;
17413 if (cartesian_waypoints_planner_B.bid_e > 0.0) {
17414 parent = rigidbodytree->Bodies[static_cast<int32_T>
17415 (cartesian_waypoints_planner_B.bid_e) - 1];
17416 } else {
17417 parent = &rigidbodytree->Base;
17418 }
17419
17420 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17421 bname->size[0] = 1;
17422 bname->size[1] = parent->NameInternal->size[1];
17423 cartes_emxEnsureCapacity_char_T(bname,
17424 cartesian_waypoints_planner_B.b_kstr_h);
17425 cartesian_waypoints_planner_B.loop_ub_m = parent->NameInternal->size[0] *
17426 parent->NameInternal->size[1] - 1;
17427 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17428 cartesian_waypoints_planner_B.b_kstr_h <=
17429 cartesian_waypoints_planner_B.loop_ub_m;
17430 cartesian_waypoints_planner_B.b_kstr_h++) {
17431 bname->data[cartesian_waypoints_planner_B.b_kstr_h] = parent->
17432 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17433 }
17434
17435 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_34, iobj_33,
17436 iobj_12);
17437 }
17438
17439 if (7.0 <= rigidbodytree->NumBodies) {
17440 body = rigidbodytree->Bodies[6];
17441 cartesian_waypoints_planner_B.bid_e = body->ParentIndex;
17442 if (cartesian_waypoints_planner_B.bid_e > 0.0) {
17443 parent = rigidbodytree->Bodies[static_cast<int32_T>
17444 (cartesian_waypoints_planner_B.bid_e) - 1];
17445 } else {
17446 parent = &rigidbodytree->Base;
17447 }
17448
17449 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17450 bname->size[0] = 1;
17451 bname->size[1] = parent->NameInternal->size[1];
17452 cartes_emxEnsureCapacity_char_T(bname,
17453 cartesian_waypoints_planner_B.b_kstr_h);
17454 cartesian_waypoints_planner_B.loop_ub_m = parent->NameInternal->size[0] *
17455 parent->NameInternal->size[1] - 1;
17456 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17457 cartesian_waypoints_planner_B.b_kstr_h <=
17458 cartesian_waypoints_planner_B.loop_ub_m;
17459 cartesian_waypoints_planner_B.b_kstr_h++) {
17460 bname->data[cartesian_waypoints_planner_B.b_kstr_h] = parent->
17461 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17462 }
17463
17464 cartesian_waypoints_planner_B.bid_e = -1.0;
17465 cartesian_waypoints_planner_B.b_kstr_h = b_basename->size[0] *
17466 b_basename->size[1];
17467 b_basename->size[0] = 1;
17468 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
17469 cartes_emxEnsureCapacity_char_T(b_basename,
17470 cartesian_waypoints_planner_B.b_kstr_h);
17471 cartesian_waypoints_planner_B.loop_ub_m = newrobot->Base.NameInternal->size
17472 [0] * newrobot->Base.NameInternal->size[1] - 1;
17473 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17474 cartesian_waypoints_planner_B.b_kstr_h <=
17475 cartesian_waypoints_planner_B.loop_ub_m;
17476 cartesian_waypoints_planner_B.b_kstr_h++) {
17477 b_basename->data[cartesian_waypoints_planner_B.b_kstr_h] =
17478 newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17479 }
17480
17481 if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {
17482 cartesian_waypoints_planner_B.bid_e = 0.0;
17483 } else {
17484 cartesian_waypoints_planner_B.b_index = newrobot->NumBodies;
17485 cartesian_waypoints_planner_B.b_i_i = 0;
17486 exitg1 = false;
17487 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_i <=
17488 static_cast<int32_T>
17489 (cartesian_waypoints_planner_B.b_index) - 1)) {
17490 body_0 = newrobot->Bodies[cartesian_waypoints_planner_B.b_i_i];
17491 cartesian_waypoints_planner_B.b_kstr_h = b_basename->size[0] *
17492 b_basename->size[1];
17493 b_basename->size[0] = 1;
17494 b_basename->size[1] = body_0->NameInternal->size[1];
17495 cartes_emxEnsureCapacity_char_T(b_basename,
17496 cartesian_waypoints_planner_B.b_kstr_h);
17497 cartesian_waypoints_planner_B.loop_ub_m = body_0->NameInternal->size[0] *
17498 body_0->NameInternal->size[1] - 1;
17499 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17500 cartesian_waypoints_planner_B.b_kstr_h <=
17501 cartesian_waypoints_planner_B.loop_ub_m;
17502 cartesian_waypoints_planner_B.b_kstr_h++) {
17503 b_basename->data[cartesian_waypoints_planner_B.b_kstr_h] =
17504 body_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17505 }
17506
17507 if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {
17508 cartesian_waypoints_planner_B.bid_e = static_cast<real_T>
17509 (cartesian_waypoints_planner_B.b_i_i) + 1.0;
17510 exitg1 = true;
17511 } else {
17512 cartesian_waypoints_planner_B.b_i_i++;
17513 }
17514 }
17515 }
17516
17517 cartesian_waypoints_planner_B.b_index = newrobot->NumBodies + 1.0;
17518 body_0 = cartesian_waypoi_RigidBody_copy(body, iobj_35, iobj_36, iobj_13);
17519 newrobot->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.b_index)
17520 - 1] = body_0;
17521 body_0->Index = cartesian_waypoints_planner_B.b_index;
17522 body_0->ParentIndex = cartesian_waypoints_planner_B.bid_e;
17523 body_0->JointInternal->InTree = true;
17524 newrobot->NumBodies++;
17525 jnt = body_0->JointInternal;
17526 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17527 bname->size[0] = 1;
17528 bname->size[1] = jnt->Type->size[1];
17529 cartes_emxEnsureCapacity_char_T(bname,
17530 cartesian_waypoints_planner_B.b_kstr_h);
17531 cartesian_waypoints_planner_B.loop_ub_m = jnt->Type->size[0] * jnt->
17532 Type->size[1] - 1;
17533 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17534 cartesian_waypoints_planner_B.b_kstr_h <=
17535 cartesian_waypoints_planner_B.loop_ub_m;
17536 cartesian_waypoints_planner_B.b_kstr_h++) {
17537 bname->data[cartesian_waypoints_planner_B.b_kstr_h] = jnt->Type->
17538 data[cartesian_waypoints_planner_B.b_kstr_h];
17539 }
17540
17541 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17542 cartesian_waypoints_planner_B.b_kstr_h < 5;
17543 cartesian_waypoints_planner_B.b_kstr_h++) {
17544 cartesian_waypoints_planner_B.b_ji[cartesian_waypoints_planner_B.b_kstr_h]
17545 = tmp[cartesian_waypoints_planner_B.b_kstr_h];
17546 }
17547
17548 cartesian_waypoints_planner_B.b_bool_m = false;
17549 if (bname->size[1] == 5) {
17550 cartesian_waypoints_planner_B.b_kstr_h = 1;
17551 do {
17552 exitg2 = 0;
17553 if (cartesian_waypoints_planner_B.b_kstr_h - 1 < 5) {
17554 cartesian_waypoints_planner_B.loop_ub_m =
17555 cartesian_waypoints_planner_B.b_kstr_h - 1;
17556 if (bname->data[cartesian_waypoints_planner_B.loop_ub_m] !=
17557 cartesian_waypoints_planner_B.b_ji[cartesian_waypoints_planner_B.loop_ub_m])
17558 {
17559 exitg2 = 1;
17560 } else {
17561 cartesian_waypoints_planner_B.b_kstr_h++;
17562 }
17563 } else {
17564 cartesian_waypoints_planner_B.b_bool_m = true;
17565 exitg2 = 1;
17566 }
17567 } while (exitg2 == 0);
17568 }
17569
17570 if (!cartesian_waypoints_planner_B.b_bool_m) {
17571 newrobot->NumNonFixedBodies++;
17572 jnt = body_0->JointInternal;
17573 cartesian_waypoints_planner_B.b_kstr_h = static_cast<int32_T>
17574 (body_0->Index) - 1;
17575 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_h] =
17576 newrobot->PositionNumber + 1.0;
17577 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_h + 8] =
17578 newrobot->PositionNumber + jnt->PositionNumber;
17579 jnt = body_0->JointInternal;
17580 cartesian_waypoints_planner_B.b_kstr_h = static_cast<int32_T>
17581 (body_0->Index) - 1;
17582 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_h] =
17583 newrobot->VelocityNumber + 1.0;
17584 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_h + 8] =
17585 newrobot->VelocityNumber + jnt->VelocityNumber;
17586 } else {
17587 cartesian_waypoints_planner_B.b_kstr_h = static_cast<int32_T>
17588 (body_0->Index);
17589 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_h - 1] = 0.0;
17590 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_h + 7] =
17591 -1.0;
17592 cartesian_waypoints_planner_B.b_kstr_h = static_cast<int32_T>
17593 (body_0->Index);
17594 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_h - 1] = 0.0;
17595 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_h + 7] =
17596 -1.0;
17597 }
17598
17599 jnt = body_0->JointInternal;
17600 newrobot->PositionNumber += jnt->PositionNumber;
17601 jnt = body_0->JointInternal;
17602 newrobot->VelocityNumber += jnt->VelocityNumber;
17603 }
17604
17605 if (8.0 <= rigidbodytree->NumBodies) {
17606 body = rigidbodytree->Bodies[7];
17607 cartesian_waypoints_planner_B.bid_e = body->ParentIndex;
17608 if (cartesian_waypoints_planner_B.bid_e > 0.0) {
17609 parent = rigidbodytree->Bodies[static_cast<int32_T>
17610 (cartesian_waypoints_planner_B.bid_e) - 1];
17611 } else {
17612 parent = &rigidbodytree->Base;
17613 }
17614
17615 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17616 bname->size[0] = 1;
17617 bname->size[1] = parent->NameInternal->size[1];
17618 cartes_emxEnsureCapacity_char_T(bname,
17619 cartesian_waypoints_planner_B.b_kstr_h);
17620 cartesian_waypoints_planner_B.loop_ub_m = parent->NameInternal->size[0] *
17621 parent->NameInternal->size[1] - 1;
17622 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17623 cartesian_waypoints_planner_B.b_kstr_h <=
17624 cartesian_waypoints_planner_B.loop_ub_m;
17625 cartesian_waypoints_planner_B.b_kstr_h++) {
17626 bname->data[cartesian_waypoints_planner_B.b_kstr_h] = parent->
17627 NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17628 }
17629
17630 cartesian_waypoints_planner_B.bid_e = -1.0;
17631 cartesian_waypoints_planner_B.b_kstr_h = b_basename->size[0] *
17632 b_basename->size[1];
17633 b_basename->size[0] = 1;
17634 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
17635 cartes_emxEnsureCapacity_char_T(b_basename,
17636 cartesian_waypoints_planner_B.b_kstr_h);
17637 cartesian_waypoints_planner_B.loop_ub_m = newrobot->Base.NameInternal->size
17638 [0] * newrobot->Base.NameInternal->size[1] - 1;
17639 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17640 cartesian_waypoints_planner_B.b_kstr_h <=
17641 cartesian_waypoints_planner_B.loop_ub_m;
17642 cartesian_waypoints_planner_B.b_kstr_h++) {
17643 b_basename->data[cartesian_waypoints_planner_B.b_kstr_h] =
17644 newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17645 }
17646
17647 if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {
17648 cartesian_waypoints_planner_B.bid_e = 0.0;
17649 } else {
17650 cartesian_waypoints_planner_B.b_index = newrobot->NumBodies;
17651 cartesian_waypoints_planner_B.b_i_i = 0;
17652 exitg1 = false;
17653 while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_i <=
17654 static_cast<int32_T>
17655 (cartesian_waypoints_planner_B.b_index) - 1)) {
17656 body_0 = newrobot->Bodies[cartesian_waypoints_planner_B.b_i_i];
17657 cartesian_waypoints_planner_B.b_kstr_h = b_basename->size[0] *
17658 b_basename->size[1];
17659 b_basename->size[0] = 1;
17660 b_basename->size[1] = body_0->NameInternal->size[1];
17661 cartes_emxEnsureCapacity_char_T(b_basename,
17662 cartesian_waypoints_planner_B.b_kstr_h);
17663 cartesian_waypoints_planner_B.loop_ub_m = body_0->NameInternal->size[0] *
17664 body_0->NameInternal->size[1] - 1;
17665 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17666 cartesian_waypoints_planner_B.b_kstr_h <=
17667 cartesian_waypoints_planner_B.loop_ub_m;
17668 cartesian_waypoints_planner_B.b_kstr_h++) {
17669 b_basename->data[cartesian_waypoints_planner_B.b_kstr_h] =
17670 body_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_h];
17671 }
17672
17673 if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {
17674 cartesian_waypoints_planner_B.bid_e = static_cast<real_T>
17675 (cartesian_waypoints_planner_B.b_i_i) + 1.0;
17676 exitg1 = true;
17677 } else {
17678 cartesian_waypoints_planner_B.b_i_i++;
17679 }
17680 }
17681 }
17682
17683 cartesian_waypoints_planner_B.b_index = newrobot->NumBodies + 1.0;
17684 body_0 = cartesian_waypoi_RigidBody_copy(body, iobj_37, iobj_38, iobj_14);
17685 newrobot->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.b_index)
17686 - 1] = body_0;
17687 body_0->Index = cartesian_waypoints_planner_B.b_index;
17688 body_0->ParentIndex = cartesian_waypoints_planner_B.bid_e;
17689 body_0->JointInternal->InTree = true;
17690 newrobot->NumBodies++;
17691 jnt = body_0->JointInternal;
17692 cartesian_waypoints_planner_B.b_kstr_h = bname->size[0] * bname->size[1];
17693 bname->size[0] = 1;
17694 bname->size[1] = jnt->Type->size[1];
17695 cartes_emxEnsureCapacity_char_T(bname,
17696 cartesian_waypoints_planner_B.b_kstr_h);
17697 cartesian_waypoints_planner_B.loop_ub_m = jnt->Type->size[0] * jnt->
17698 Type->size[1] - 1;
17699 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17700 cartesian_waypoints_planner_B.b_kstr_h <=
17701 cartesian_waypoints_planner_B.loop_ub_m;
17702 cartesian_waypoints_planner_B.b_kstr_h++) {
17703 bname->data[cartesian_waypoints_planner_B.b_kstr_h] = jnt->Type->
17704 data[cartesian_waypoints_planner_B.b_kstr_h];
17705 }
17706
17707 for (cartesian_waypoints_planner_B.b_kstr_h = 0;
17708 cartesian_waypoints_planner_B.b_kstr_h < 5;
17709 cartesian_waypoints_planner_B.b_kstr_h++) {
17710 cartesian_waypoints_planner_B.b_ji[cartesian_waypoints_planner_B.b_kstr_h]
17711 = tmp[cartesian_waypoints_planner_B.b_kstr_h];
17712 }
17713
17714 cartesian_waypoints_planner_B.b_bool_m = false;
17715 if (bname->size[1] == 5) {
17716 cartesian_waypoints_planner_B.b_kstr_h = 1;
17717 do {
17718 exitg2 = 0;
17719 if (cartesian_waypoints_planner_B.b_kstr_h - 1 < 5) {
17720 cartesian_waypoints_planner_B.loop_ub_m =
17721 cartesian_waypoints_planner_B.b_kstr_h - 1;
17722 if (bname->data[cartesian_waypoints_planner_B.loop_ub_m] !=
17723 cartesian_waypoints_planner_B.b_ji[cartesian_waypoints_planner_B.loop_ub_m])
17724 {
17725 exitg2 = 1;
17726 } else {
17727 cartesian_waypoints_planner_B.b_kstr_h++;
17728 }
17729 } else {
17730 cartesian_waypoints_planner_B.b_bool_m = true;
17731 exitg2 = 1;
17732 }
17733 } while (exitg2 == 0);
17734 }
17735
17736 if (!cartesian_waypoints_planner_B.b_bool_m) {
17737 newrobot->NumNonFixedBodies++;
17738 jnt = body_0->JointInternal;
17739 cartesian_waypoints_planner_B.b_kstr_h = static_cast<int32_T>
17740 (body_0->Index) - 1;
17741 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_h] =
17742 newrobot->PositionNumber + 1.0;
17743 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_h + 8] =
17744 newrobot->PositionNumber + jnt->PositionNumber;
17745 jnt = body_0->JointInternal;
17746 cartesian_waypoints_planner_B.b_kstr_h = static_cast<int32_T>
17747 (body_0->Index) - 1;
17748 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_h] =
17749 newrobot->VelocityNumber + 1.0;
17750 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_h + 8] =
17751 newrobot->VelocityNumber + jnt->VelocityNumber;
17752 } else {
17753 cartesian_waypoints_planner_B.b_kstr_h = static_cast<int32_T>
17754 (body_0->Index);
17755 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_h - 1] = 0.0;
17756 newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_h + 7] =
17757 -1.0;
17758 cartesian_waypoints_planner_B.b_kstr_h = static_cast<int32_T>
17759 (body_0->Index);
17760 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_h - 1] = 0.0;
17761 newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_h + 7] =
17762 -1.0;
17763 }
17764
17765 jnt = body_0->JointInternal;
17766 newrobot->PositionNumber += jnt->PositionNumber;
17767 jnt = body_0->JointInternal;
17768 newrobot->VelocityNumber += jnt->VelocityNumber;
17769 }
17770
17771 cartesian_waypoi_emxFree_char_T(&bname);
17772 cartesian_waypoi_emxFree_char_T(&b_basename);
17773 obj->RigidBodyTreeInternal = newrobot;
17774}
17775
17776static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
17777 (h_robotics_core_internal_Damp_T *obj)
17778{
17779 h_robotics_core_internal_Damp_T *b_obj;
17780 int32_T i;
17781 static const char_T tmp[22] = { 'B', 'F', 'G', 'S', 'G', 'r', 'a', 'd', 'i',
17782 'e', 'n', 't', 'P', 'r', 'o', 'j', 'e', 'c', 't', 'i', 'o', 'n' };
17783
17784 b_obj = obj;
17785 obj->MaxNumIteration = 1500.0;
17786 obj->MaxTime = 10.0;
17787 obj->GradientTolerance = 1.0E-7;
17788 obj->SolutionTolerance = 1.0E-6;
17789 obj->ArmijoRuleBeta = 0.4;
17790 obj->ArmijoRuleSigma = 1.0E-5;
17791 obj->ConstraintsOn = true;
17792 obj->RandomRestart = true;
17793 obj->StepTolerance = 1.0E-14;
17794 for (i = 0; i < 22; i++) {
17795 obj->Name[i] = tmp[i];
17796 }
17797
17798 obj->ConstraintMatrix->size[0] = 0;
17799 obj->ConstraintMatrix->size[1] = 0;
17800 obj->ConstraintBound->size[0] = 0;
17801 obj->TimeObj.StartTime = -1.0;
17802 obj->TimeObjInternal.StartTime = -1.0;
17803 return b_obj;
17804}
17805
17806static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_wa_T
17807 *pStruct)
17808{
17809 cartesian_waypoi_emxInit_char_T(&pStruct->Type, 2);
17810}
17811
17812static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
17813 *pStruct)
17814{
17815 cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);
17816 emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
17817}
17818
17819static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
17820 *pStruct)
17821{
17822 emxInitStruct_o_robotics_manip_(&pStruct->Base);
17823}
17824
17825static void emxInitStruct_robotics_slmani_e(robotics_slmanip_internal_blo_T
17826 *pStruct)
17827{
17828 emxInitStruct_p_robotics_manip_(&pStruct->TreeInternal);
17829}
17830
17831static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
17832 *pStruct)
17833{
17834 cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);
17835 emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
17836}
17837
17838static n_robotics_manip_internal_Rig_T *cartesian_w_RigidBody_RigidBody
17839 (n_robotics_manip_internal_Rig_T *obj)
17840{
17841 n_robotics_manip_internal_Rig_T *b_obj;
17842 emxArray_char_T_cartesian_way_T *switch_expression;
17843 boolean_T b_bool;
17844 int32_T b_kstr;
17845 char_T b[8];
17846 char_T b_0[9];
17847 int32_T loop_ub;
17848 static const char_T tmp[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
17849 'l', 'i', 'n', 'k' };
17850
17851 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
17852
17853 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17854
17855 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17856
17857 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
17858 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
17859
17860 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
17861 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
17862
17863 int32_T exitg1;
17864 b_obj = obj;
17865 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
17866 obj->NameInternal->size[0] = 1;
17867 obj->NameInternal->size[1] = 13;
17868 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
17869 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
17870 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
17871 }
17872
17873 obj->ParentIndex = 0.0;
17874 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
17875 obj->JointInternal.Type->size[0] = 1;
17876 obj->JointInternal.Type->size[1] = 5;
17877 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
17878 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
17879 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
17880 }
17881
17882 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
17883 b_kstr = switch_expression->size[0] * switch_expression->size[1];
17884 switch_expression->size[0] = 1;
17885 switch_expression->size[1] = obj->JointInternal.Type->size[1];
17886 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
17887 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
17888 - 1;
17889 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
17890 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
17891 }
17892
17893 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
17894 b[b_kstr] = tmp_1[b_kstr];
17895 }
17896
17897 b_bool = false;
17898 if (switch_expression->size[1] == 8) {
17899 b_kstr = 1;
17900 do {
17901 exitg1 = 0;
17902 if (b_kstr - 1 < 8) {
17903 loop_ub = b_kstr - 1;
17904 if (switch_expression->data[loop_ub] != b[loop_ub]) {
17905 exitg1 = 1;
17906 } else {
17907 b_kstr++;
17908 }
17909 } else {
17910 b_bool = true;
17911 exitg1 = 1;
17912 }
17913 } while (exitg1 == 0);
17914 }
17915
17916 if (b_bool) {
17917 b_kstr = 0;
17918 } else {
17919 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
17920 b_0[b_kstr] = tmp_2[b_kstr];
17921 }
17922
17923 b_bool = false;
17924 if (switch_expression->size[1] == 9) {
17925 b_kstr = 1;
17926 do {
17927 exitg1 = 0;
17928 if (b_kstr - 1 < 9) {
17929 loop_ub = b_kstr - 1;
17930 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
17931 exitg1 = 1;
17932 } else {
17933 b_kstr++;
17934 }
17935 } else {
17936 b_bool = true;
17937 exitg1 = 1;
17938 }
17939 } while (exitg1 == 0);
17940 }
17941
17942 if (b_bool) {
17943 b_kstr = 1;
17944 } else {
17945 b_kstr = -1;
17946 }
17947 }
17948
17949 cartesian_waypoi_emxFree_char_T(&switch_expression);
17950 switch (b_kstr) {
17951 case 0:
17952 obj->JointInternal.PositionNumber = 1.0;
17953 obj->JointInternal.JointAxisInternal[0] = 0.0;
17954 obj->JointInternal.JointAxisInternal[1] = 0.0;
17955 obj->JointInternal.JointAxisInternal[2] = 1.0;
17956 break;
17957
17958 case 1:
17959 obj->JointInternal.PositionNumber = 1.0;
17960 obj->JointInternal.JointAxisInternal[0] = 0.0;
17961 obj->JointInternal.JointAxisInternal[1] = 0.0;
17962 obj->JointInternal.JointAxisInternal[2] = 1.0;
17963 break;
17964
17965 default:
17966 obj->JointInternal.PositionNumber = 0.0;
17967 obj->JointInternal.JointAxisInternal[0] = 0.0;
17968 obj->JointInternal.JointAxisInternal[1] = 0.0;
17969 obj->JointInternal.JointAxisInternal[2] = 0.0;
17970 break;
17971 }
17972
17973 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17974 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
17975 }
17976
17977 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
17978 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
17979 }
17980
17981 obj->JointInternal.JointAxisInternal[0] = 0.0;
17982 obj->JointInternal.JointAxisInternal[1] = 0.0;
17983 obj->JointInternal.JointAxisInternal[2] = 0.0;
17984 return b_obj;
17985}
17986
17987static n_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_e
17988 (n_robotics_manip_internal_Rig_T *obj)
17989{
17990 n_robotics_manip_internal_Rig_T *b_obj;
17991 emxArray_char_T_cartesian_way_T *switch_expression;
17992 boolean_T b_bool;
17993 int32_T b_kstr;
17994 char_T b[8];
17995 char_T b_0[9];
17996 int32_T loop_ub;
17997 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
17998 '1' };
17999
18000 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18001
18002 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18003
18004 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
18005 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
18006
18007 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
18008 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
18009
18010 int32_T exitg1;
18011 b_obj = obj;
18012 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
18013 obj->NameInternal->size[0] = 1;
18014 obj->NameInternal->size[1] = 10;
18015 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
18016 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
18017 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
18018 }
18019
18020 obj->ParentIndex = 1.0;
18021 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
18022 obj->JointInternal.Type->size[0] = 1;
18023 obj->JointInternal.Type->size[1] = 8;
18024 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
18025 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18026 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
18027 }
18028
18029 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
18030 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18031 switch_expression->size[0] = 1;
18032 switch_expression->size[1] = obj->JointInternal.Type->size[1];
18033 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18034 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
18035 - 1;
18036 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18037 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
18038 }
18039
18040 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18041 b[b_kstr] = tmp_0[b_kstr];
18042 }
18043
18044 b_bool = false;
18045 if (switch_expression->size[1] == 8) {
18046 b_kstr = 1;
18047 do {
18048 exitg1 = 0;
18049 if (b_kstr - 1 < 8) {
18050 loop_ub = b_kstr - 1;
18051 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18052 exitg1 = 1;
18053 } else {
18054 b_kstr++;
18055 }
18056 } else {
18057 b_bool = true;
18058 exitg1 = 1;
18059 }
18060 } while (exitg1 == 0);
18061 }
18062
18063 if (b_bool) {
18064 b_kstr = 0;
18065 } else {
18066 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18067 b_0[b_kstr] = tmp_1[b_kstr];
18068 }
18069
18070 b_bool = false;
18071 if (switch_expression->size[1] == 9) {
18072 b_kstr = 1;
18073 do {
18074 exitg1 = 0;
18075 if (b_kstr - 1 < 9) {
18076 loop_ub = b_kstr - 1;
18077 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
18078 exitg1 = 1;
18079 } else {
18080 b_kstr++;
18081 }
18082 } else {
18083 b_bool = true;
18084 exitg1 = 1;
18085 }
18086 } while (exitg1 == 0);
18087 }
18088
18089 if (b_bool) {
18090 b_kstr = 1;
18091 } else {
18092 b_kstr = -1;
18093 }
18094 }
18095
18096 cartesian_waypoi_emxFree_char_T(&switch_expression);
18097 switch (b_kstr) {
18098 case 0:
18099 obj->JointInternal.PositionNumber = 1.0;
18100 obj->JointInternal.JointAxisInternal[0] = 0.0;
18101 obj->JointInternal.JointAxisInternal[1] = 0.0;
18102 obj->JointInternal.JointAxisInternal[2] = 1.0;
18103 break;
18104
18105 case 1:
18106 obj->JointInternal.PositionNumber = 1.0;
18107 obj->JointInternal.JointAxisInternal[0] = 0.0;
18108 obj->JointInternal.JointAxisInternal[1] = 0.0;
18109 obj->JointInternal.JointAxisInternal[2] = 1.0;
18110 break;
18111
18112 default:
18113 obj->JointInternal.PositionNumber = 0.0;
18114 obj->JointInternal.JointAxisInternal[0] = 0.0;
18115 obj->JointInternal.JointAxisInternal[1] = 0.0;
18116 obj->JointInternal.JointAxisInternal[2] = 0.0;
18117 break;
18118 }
18119
18120 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18121 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
18122 }
18123
18124 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18125 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
18126 }
18127
18128 obj->JointInternal.JointAxisInternal[0] = 0.0;
18129 obj->JointInternal.JointAxisInternal[1] = 0.0;
18130 obj->JointInternal.JointAxisInternal[2] = 1.0;
18131 return b_obj;
18132}
18133
18134static n_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_ev
18135 (n_robotics_manip_internal_Rig_T *obj)
18136{
18137 n_robotics_manip_internal_Rig_T *b_obj;
18138 emxArray_char_T_cartesian_way_T *switch_expression;
18139 boolean_T b_bool;
18140 int32_T b_kstr;
18141 char_T b[8];
18142 char_T b_0[9];
18143 int32_T loop_ub;
18144 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
18145 '2' };
18146
18147 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18148
18149 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18150
18151 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
18152 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
18153 0.0, 0.0, 0.0, 1.0 };
18154
18155 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
18156 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
18157
18158 int32_T exitg1;
18159 b_obj = obj;
18160 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
18161 obj->NameInternal->size[0] = 1;
18162 obj->NameInternal->size[1] = 10;
18163 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
18164 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
18165 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
18166 }
18167
18168 obj->ParentIndex = 2.0;
18169 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
18170 obj->JointInternal.Type->size[0] = 1;
18171 obj->JointInternal.Type->size[1] = 8;
18172 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
18173 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18174 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
18175 }
18176
18177 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
18178 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18179 switch_expression->size[0] = 1;
18180 switch_expression->size[1] = obj->JointInternal.Type->size[1];
18181 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18182 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
18183 - 1;
18184 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18185 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
18186 }
18187
18188 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18189 b[b_kstr] = tmp_0[b_kstr];
18190 }
18191
18192 b_bool = false;
18193 if (switch_expression->size[1] == 8) {
18194 b_kstr = 1;
18195 do {
18196 exitg1 = 0;
18197 if (b_kstr - 1 < 8) {
18198 loop_ub = b_kstr - 1;
18199 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18200 exitg1 = 1;
18201 } else {
18202 b_kstr++;
18203 }
18204 } else {
18205 b_bool = true;
18206 exitg1 = 1;
18207 }
18208 } while (exitg1 == 0);
18209 }
18210
18211 if (b_bool) {
18212 b_kstr = 0;
18213 } else {
18214 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18215 b_0[b_kstr] = tmp_1[b_kstr];
18216 }
18217
18218 b_bool = false;
18219 if (switch_expression->size[1] == 9) {
18220 b_kstr = 1;
18221 do {
18222 exitg1 = 0;
18223 if (b_kstr - 1 < 9) {
18224 loop_ub = b_kstr - 1;
18225 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
18226 exitg1 = 1;
18227 } else {
18228 b_kstr++;
18229 }
18230 } else {
18231 b_bool = true;
18232 exitg1 = 1;
18233 }
18234 } while (exitg1 == 0);
18235 }
18236
18237 if (b_bool) {
18238 b_kstr = 1;
18239 } else {
18240 b_kstr = -1;
18241 }
18242 }
18243
18244 cartesian_waypoi_emxFree_char_T(&switch_expression);
18245 switch (b_kstr) {
18246 case 0:
18247 obj->JointInternal.PositionNumber = 1.0;
18248 obj->JointInternal.JointAxisInternal[0] = 0.0;
18249 obj->JointInternal.JointAxisInternal[1] = 0.0;
18250 obj->JointInternal.JointAxisInternal[2] = 1.0;
18251 break;
18252
18253 case 1:
18254 obj->JointInternal.PositionNumber = 1.0;
18255 obj->JointInternal.JointAxisInternal[0] = 0.0;
18256 obj->JointInternal.JointAxisInternal[1] = 0.0;
18257 obj->JointInternal.JointAxisInternal[2] = 1.0;
18258 break;
18259
18260 default:
18261 obj->JointInternal.PositionNumber = 0.0;
18262 obj->JointInternal.JointAxisInternal[0] = 0.0;
18263 obj->JointInternal.JointAxisInternal[1] = 0.0;
18264 obj->JointInternal.JointAxisInternal[2] = 0.0;
18265 break;
18266 }
18267
18268 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18269 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
18270 }
18271
18272 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18273 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
18274 }
18275
18276 obj->JointInternal.JointAxisInternal[0] = 0.0;
18277 obj->JointInternal.JointAxisInternal[1] = 0.0;
18278 obj->JointInternal.JointAxisInternal[2] = -1.0;
18279 return b_obj;
18280}
18281
18282static n_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_evq
18283 (n_robotics_manip_internal_Rig_T *obj)
18284{
18285 n_robotics_manip_internal_Rig_T *b_obj;
18286 emxArray_char_T_cartesian_way_T *switch_expression;
18287 boolean_T b_bool;
18288 int32_T b_kstr;
18289 char_T b[8];
18290 char_T b_0[9];
18291 int32_T loop_ub;
18292 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
18293 '3' };
18294
18295 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18296
18297 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18298
18299 static const real_T tmp_2[16] = { 1.0, 2.0682310711021444E-13,
18300 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
18301 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
18302 1.0 };
18303
18304 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
18305 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
18306
18307 int32_T exitg1;
18308 b_obj = obj;
18309 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
18310 obj->NameInternal->size[0] = 1;
18311 obj->NameInternal->size[1] = 10;
18312 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
18313 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
18314 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
18315 }
18316
18317 obj->ParentIndex = 3.0;
18318 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
18319 obj->JointInternal.Type->size[0] = 1;
18320 obj->JointInternal.Type->size[1] = 8;
18321 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
18322 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18323 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
18324 }
18325
18326 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
18327 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18328 switch_expression->size[0] = 1;
18329 switch_expression->size[1] = obj->JointInternal.Type->size[1];
18330 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18331 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
18332 - 1;
18333 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18334 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
18335 }
18336
18337 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18338 b[b_kstr] = tmp_0[b_kstr];
18339 }
18340
18341 b_bool = false;
18342 if (switch_expression->size[1] == 8) {
18343 b_kstr = 1;
18344 do {
18345 exitg1 = 0;
18346 if (b_kstr - 1 < 8) {
18347 loop_ub = b_kstr - 1;
18348 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18349 exitg1 = 1;
18350 } else {
18351 b_kstr++;
18352 }
18353 } else {
18354 b_bool = true;
18355 exitg1 = 1;
18356 }
18357 } while (exitg1 == 0);
18358 }
18359
18360 if (b_bool) {
18361 b_kstr = 0;
18362 } else {
18363 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18364 b_0[b_kstr] = tmp_1[b_kstr];
18365 }
18366
18367 b_bool = false;
18368 if (switch_expression->size[1] == 9) {
18369 b_kstr = 1;
18370 do {
18371 exitg1 = 0;
18372 if (b_kstr - 1 < 9) {
18373 loop_ub = b_kstr - 1;
18374 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
18375 exitg1 = 1;
18376 } else {
18377 b_kstr++;
18378 }
18379 } else {
18380 b_bool = true;
18381 exitg1 = 1;
18382 }
18383 } while (exitg1 == 0);
18384 }
18385
18386 if (b_bool) {
18387 b_kstr = 1;
18388 } else {
18389 b_kstr = -1;
18390 }
18391 }
18392
18393 cartesian_waypoi_emxFree_char_T(&switch_expression);
18394 switch (b_kstr) {
18395 case 0:
18396 obj->JointInternal.PositionNumber = 1.0;
18397 obj->JointInternal.JointAxisInternal[0] = 0.0;
18398 obj->JointInternal.JointAxisInternal[1] = 0.0;
18399 obj->JointInternal.JointAxisInternal[2] = 1.0;
18400 break;
18401
18402 case 1:
18403 obj->JointInternal.PositionNumber = 1.0;
18404 obj->JointInternal.JointAxisInternal[0] = 0.0;
18405 obj->JointInternal.JointAxisInternal[1] = 0.0;
18406 obj->JointInternal.JointAxisInternal[2] = 1.0;
18407 break;
18408
18409 default:
18410 obj->JointInternal.PositionNumber = 0.0;
18411 obj->JointInternal.JointAxisInternal[0] = 0.0;
18412 obj->JointInternal.JointAxisInternal[1] = 0.0;
18413 obj->JointInternal.JointAxisInternal[2] = 0.0;
18414 break;
18415 }
18416
18417 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18418 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
18419 }
18420
18421 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18422 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
18423 }
18424
18425 obj->JointInternal.JointAxisInternal[0] = 0.0;
18426 obj->JointInternal.JointAxisInternal[1] = 0.0;
18427 obj->JointInternal.JointAxisInternal[2] = 1.0;
18428 return b_obj;
18429}
18430
18431static n_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_evqu
18432 (n_robotics_manip_internal_Rig_T *obj)
18433{
18434 n_robotics_manip_internal_Rig_T *b_obj;
18435 emxArray_char_T_cartesian_way_T *switch_expression;
18436 boolean_T b_bool;
18437 int32_T b_kstr;
18438 char_T b[8];
18439 char_T b_0[9];
18440 int32_T loop_ub;
18441 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
18442 '4' };
18443
18444 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18445
18446 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18447
18448 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
18449 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
18450 0.0, -0.268, 0.0, 1.0 };
18451
18452 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
18453 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
18454
18455 int32_T exitg1;
18456 b_obj = obj;
18457 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
18458 obj->NameInternal->size[0] = 1;
18459 obj->NameInternal->size[1] = 10;
18460 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
18461 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
18462 obj->NameInternal->data[b_kstr] = tmp[b_kstr];
18463 }
18464
18465 obj->ParentIndex = 4.0;
18466 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
18467 obj->JointInternal.Type->size[0] = 1;
18468 obj->JointInternal.Type->size[1] = 8;
18469 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
18470 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18471 obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
18472 }
18473
18474 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
18475 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18476 switch_expression->size[0] = 1;
18477 switch_expression->size[1] = obj->JointInternal.Type->size[1];
18478 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18479 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
18480 - 1;
18481 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18482 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
18483 }
18484
18485 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18486 b[b_kstr] = tmp_0[b_kstr];
18487 }
18488
18489 b_bool = false;
18490 if (switch_expression->size[1] == 8) {
18491 b_kstr = 1;
18492 do {
18493 exitg1 = 0;
18494 if (b_kstr - 1 < 8) {
18495 loop_ub = b_kstr - 1;
18496 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18497 exitg1 = 1;
18498 } else {
18499 b_kstr++;
18500 }
18501 } else {
18502 b_bool = true;
18503 exitg1 = 1;
18504 }
18505 } while (exitg1 == 0);
18506 }
18507
18508 if (b_bool) {
18509 b_kstr = 0;
18510 } else {
18511 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18512 b_0[b_kstr] = tmp_1[b_kstr];
18513 }
18514
18515 b_bool = false;
18516 if (switch_expression->size[1] == 9) {
18517 b_kstr = 1;
18518 do {
18519 exitg1 = 0;
18520 if (b_kstr - 1 < 9) {
18521 loop_ub = b_kstr - 1;
18522 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
18523 exitg1 = 1;
18524 } else {
18525 b_kstr++;
18526 }
18527 } else {
18528 b_bool = true;
18529 exitg1 = 1;
18530 }
18531 } while (exitg1 == 0);
18532 }
18533
18534 if (b_bool) {
18535 b_kstr = 1;
18536 } else {
18537 b_kstr = -1;
18538 }
18539 }
18540
18541 cartesian_waypoi_emxFree_char_T(&switch_expression);
18542 switch (b_kstr) {
18543 case 0:
18544 obj->JointInternal.PositionNumber = 1.0;
18545 obj->JointInternal.JointAxisInternal[0] = 0.0;
18546 obj->JointInternal.JointAxisInternal[1] = 0.0;
18547 obj->JointInternal.JointAxisInternal[2] = 1.0;
18548 break;
18549
18550 case 1:
18551 obj->JointInternal.PositionNumber = 1.0;
18552 obj->JointInternal.JointAxisInternal[0] = 0.0;
18553 obj->JointInternal.JointAxisInternal[1] = 0.0;
18554 obj->JointInternal.JointAxisInternal[2] = 1.0;
18555 break;
18556
18557 default:
18558 obj->JointInternal.PositionNumber = 0.0;
18559 obj->JointInternal.JointAxisInternal[0] = 0.0;
18560 obj->JointInternal.JointAxisInternal[1] = 0.0;
18561 obj->JointInternal.JointAxisInternal[2] = 0.0;
18562 break;
18563 }
18564
18565 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18566 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
18567 }
18568
18569 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18570 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
18571 }
18572
18573 obj->JointInternal.JointAxisInternal[0] = 0.0;
18574 obj->JointInternal.JointAxisInternal[1] = 0.0;
18575 obj->JointInternal.JointAxisInternal[2] = 1.0;
18576 return b_obj;
18577}
18578
18579static p_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
18580 (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,
18581 n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T
18582 *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,
18583 n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T
18584 *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,
18585 n_robotics_manip_internal_Rig_T *iobj_7)
18586{
18587 p_robotics_manip_internal_Rig_T *b_obj;
18588 o_robotics_manip_internal_Rig_T *obj_0;
18589 emxArray_char_T_cartesian_way_T *switch_expression;
18590 boolean_T b_bool;
18591 int32_T b_kstr;
18592 char_T b[8];
18593 char_T b_0[9];
18594 int32_T loop_ub;
18595 static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
18596 '5' };
18597
18598 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18599
18600 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18601
18602 static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
18603 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
18604
18605 static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
18606 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
18607
18608 static const char_T tmp_4[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
18609 '6' };
18610
18611 static const real_T tmp_5[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
18612 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
18613
18614 static const char_T tmp_6[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
18615 'e', 'e' };
18616
18617 static const char_T tmp_7[5] = { 'f', 'i', 'x', 'e', 'd' };
18618
18619 static const char_T tmp_8[5] = { 'w', 'o', 'r', 'l', 'd' };
18620
18621 int32_T exitg1;
18622 b_obj = obj;
18623 obj->Bodies[0] = cartesian_w_RigidBody_RigidBody(iobj_0);
18624 obj->Bodies[1] = cartesian_RigidBody_RigidBody_e(iobj_7);
18625 obj->Bodies[2] = cartesia_RigidBody_RigidBody_ev(iobj_1);
18626 obj->Bodies[3] = cartesi_RigidBody_RigidBody_evq(iobj_2);
18627 obj->Bodies[4] = cartes_RigidBody_RigidBody_evqu(iobj_3);
18628 b_kstr = iobj_4->NameInternal->size[0] * iobj_4->NameInternal->size[1];
18629 iobj_4->NameInternal->size[0] = 1;
18630 iobj_4->NameInternal->size[1] = 10;
18631 cartes_emxEnsureCapacity_char_T(iobj_4->NameInternal, b_kstr);
18632 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
18633 iobj_4->NameInternal->data[b_kstr] = tmp[b_kstr];
18634 }
18635
18636 iobj_4->ParentIndex = 5.0;
18637 b_kstr = iobj_4->JointInternal.Type->size[0] * iobj_4->
18638 JointInternal.Type->size[1];
18639 iobj_4->JointInternal.Type->size[0] = 1;
18640 iobj_4->JointInternal.Type->size[1] = 8;
18641 cartes_emxEnsureCapacity_char_T(iobj_4->JointInternal.Type, b_kstr);
18642 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18643 iobj_4->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
18644 }
18645
18646 cartesian_waypoi_emxInit_char_T(&switch_expression, 2);
18647 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18648 switch_expression->size[0] = 1;
18649 switch_expression->size[1] = iobj_4->JointInternal.Type->size[1];
18650 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18651 loop_ub = iobj_4->JointInternal.Type->size[0] * iobj_4->
18652 JointInternal.Type->size[1] - 1;
18653 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18654 switch_expression->data[b_kstr] = iobj_4->JointInternal.Type->data[b_kstr];
18655 }
18656
18657 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18658 b[b_kstr] = tmp_0[b_kstr];
18659 }
18660
18661 b_bool = false;
18662 if (switch_expression->size[1] == 8) {
18663 b_kstr = 1;
18664 do {
18665 exitg1 = 0;
18666 if (b_kstr - 1 < 8) {
18667 loop_ub = b_kstr - 1;
18668 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18669 exitg1 = 1;
18670 } else {
18671 b_kstr++;
18672 }
18673 } else {
18674 b_bool = true;
18675 exitg1 = 1;
18676 }
18677 } while (exitg1 == 0);
18678 }
18679
18680 if (b_bool) {
18681 b_kstr = 0;
18682 } else {
18683 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18684 b_0[b_kstr] = tmp_1[b_kstr];
18685 }
18686
18687 b_bool = false;
18688 if (switch_expression->size[1] == 9) {
18689 b_kstr = 1;
18690 do {
18691 exitg1 = 0;
18692 if (b_kstr - 1 < 9) {
18693 loop_ub = b_kstr - 1;
18694 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
18695 exitg1 = 1;
18696 } else {
18697 b_kstr++;
18698 }
18699 } else {
18700 b_bool = true;
18701 exitg1 = 1;
18702 }
18703 } while (exitg1 == 0);
18704 }
18705
18706 if (b_bool) {
18707 b_kstr = 1;
18708 } else {
18709 b_kstr = -1;
18710 }
18711 }
18712
18713 switch (b_kstr) {
18714 case 0:
18715 iobj_4->JointInternal.PositionNumber = 1.0;
18716 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
18717 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
18718 iobj_4->JointInternal.JointAxisInternal[2] = 1.0;
18719 break;
18720
18721 case 1:
18722 iobj_4->JointInternal.PositionNumber = 1.0;
18723 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
18724 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
18725 iobj_4->JointInternal.JointAxisInternal[2] = 1.0;
18726 break;
18727
18728 default:
18729 iobj_4->JointInternal.PositionNumber = 0.0;
18730 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
18731 iobj_4->JointInternal.JointAxisInternal[1] = 0.0;
18732 iobj_4->JointInternal.JointAxisInternal[2] = 0.0;
18733 break;
18734 }
18735
18736 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18737 iobj_4->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
18738 }
18739
18740 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18741 iobj_4->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
18742 }
18743
18744 iobj_4->JointInternal.JointAxisInternal[0] = 0.0;
18745 iobj_4->JointInternal.JointAxisInternal[1] = 1.0;
18746 iobj_4->JointInternal.JointAxisInternal[2] = 0.0;
18747 obj->Bodies[5] = iobj_4;
18748 b_kstr = iobj_5->NameInternal->size[0] * iobj_5->NameInternal->size[1];
18749 iobj_5->NameInternal->size[0] = 1;
18750 iobj_5->NameInternal->size[1] = 10;
18751 cartes_emxEnsureCapacity_char_T(iobj_5->NameInternal, b_kstr);
18752 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
18753 iobj_5->NameInternal->data[b_kstr] = tmp_4[b_kstr];
18754 }
18755
18756 iobj_5->ParentIndex = 6.0;
18757 b_kstr = iobj_5->JointInternal.Type->size[0] * iobj_5->
18758 JointInternal.Type->size[1];
18759 iobj_5->JointInternal.Type->size[0] = 1;
18760 iobj_5->JointInternal.Type->size[1] = 8;
18761 cartes_emxEnsureCapacity_char_T(iobj_5->JointInternal.Type, b_kstr);
18762 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
18763 iobj_5->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];
18764 }
18765
18766 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18767 switch_expression->size[0] = 1;
18768 switch_expression->size[1] = iobj_5->JointInternal.Type->size[1];
18769 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18770 loop_ub = iobj_5->JointInternal.Type->size[0] * iobj_5->
18771 JointInternal.Type->size[1] - 1;
18772 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18773 switch_expression->data[b_kstr] = iobj_5->JointInternal.Type->data[b_kstr];
18774 }
18775
18776 b_bool = false;
18777 if (switch_expression->size[1] == 8) {
18778 b_kstr = 1;
18779 do {
18780 exitg1 = 0;
18781 if (b_kstr - 1 < 8) {
18782 loop_ub = b_kstr - 1;
18783 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18784 exitg1 = 1;
18785 } else {
18786 b_kstr++;
18787 }
18788 } else {
18789 b_bool = true;
18790 exitg1 = 1;
18791 }
18792 } while (exitg1 == 0);
18793 }
18794
18795 if (b_bool) {
18796 b_kstr = 0;
18797 } else {
18798 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18799 b_0[b_kstr] = tmp_1[b_kstr];
18800 }
18801
18802 b_bool = false;
18803 if (switch_expression->size[1] == 9) {
18804 b_kstr = 1;
18805 do {
18806 exitg1 = 0;
18807 if (b_kstr - 1 < 9) {
18808 loop_ub = b_kstr - 1;
18809 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
18810 exitg1 = 1;
18811 } else {
18812 b_kstr++;
18813 }
18814 } else {
18815 b_bool = true;
18816 exitg1 = 1;
18817 }
18818 } while (exitg1 == 0);
18819 }
18820
18821 if (b_bool) {
18822 b_kstr = 1;
18823 } else {
18824 b_kstr = -1;
18825 }
18826 }
18827
18828 switch (b_kstr) {
18829 case 0:
18830 iobj_5->JointInternal.PositionNumber = 1.0;
18831 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
18832 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
18833 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
18834 break;
18835
18836 case 1:
18837 iobj_5->JointInternal.PositionNumber = 1.0;
18838 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
18839 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
18840 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
18841 break;
18842
18843 default:
18844 iobj_5->JointInternal.PositionNumber = 0.0;
18845 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
18846 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
18847 iobj_5->JointInternal.JointAxisInternal[2] = 0.0;
18848 break;
18849 }
18850
18851 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18852 iobj_5->JointInternal.JointToParentTransform[b_kstr] = tmp_5[b_kstr];
18853 }
18854
18855 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18856 iobj_5->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
18857 }
18858
18859 iobj_5->JointInternal.JointAxisInternal[0] = 0.0;
18860 iobj_5->JointInternal.JointAxisInternal[1] = 0.0;
18861 iobj_5->JointInternal.JointAxisInternal[2] = 1.0;
18862 obj->Bodies[6] = iobj_5;
18863 b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];
18864 iobj_6->NameInternal->size[0] = 1;
18865 iobj_6->NameInternal->size[1] = 11;
18866 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, b_kstr);
18867 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
18868 iobj_6->NameInternal->data[b_kstr] = tmp_6[b_kstr];
18869 }
18870
18871 iobj_6->ParentIndex = 7.0;
18872 b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->
18873 JointInternal.Type->size[1];
18874 iobj_6->JointInternal.Type->size[0] = 1;
18875 iobj_6->JointInternal.Type->size[1] = 5;
18876 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.Type, b_kstr);
18877 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
18878 iobj_6->JointInternal.Type->data[b_kstr] = tmp_7[b_kstr];
18879 }
18880
18881 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18882 switch_expression->size[0] = 1;
18883 switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];
18884 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
18885 loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->
18886 JointInternal.Type->size[1] - 1;
18887 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
18888 switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];
18889 }
18890
18891 b_bool = false;
18892 if (switch_expression->size[1] == 8) {
18893 b_kstr = 1;
18894 do {
18895 exitg1 = 0;
18896 if (b_kstr - 1 < 8) {
18897 loop_ub = b_kstr - 1;
18898 if (switch_expression->data[loop_ub] != b[loop_ub]) {
18899 exitg1 = 1;
18900 } else {
18901 b_kstr++;
18902 }
18903 } else {
18904 b_bool = true;
18905 exitg1 = 1;
18906 }
18907 } while (exitg1 == 0);
18908 }
18909
18910 if (b_bool) {
18911 b_kstr = 0;
18912 } else {
18913 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
18914 b_0[b_kstr] = tmp_1[b_kstr];
18915 }
18916
18917 b_bool = false;
18918 if (switch_expression->size[1] == 9) {
18919 b_kstr = 1;
18920 do {
18921 exitg1 = 0;
18922 if (b_kstr - 1 < 9) {
18923 loop_ub = b_kstr - 1;
18924 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
18925 exitg1 = 1;
18926 } else {
18927 b_kstr++;
18928 }
18929 } else {
18930 b_bool = true;
18931 exitg1 = 1;
18932 }
18933 } while (exitg1 == 0);
18934 }
18935
18936 if (b_bool) {
18937 b_kstr = 1;
18938 } else {
18939 b_kstr = -1;
18940 }
18941 }
18942
18943 switch (b_kstr) {
18944 case 0:
18945 iobj_6->JointInternal.PositionNumber = 1.0;
18946 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
18947 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
18948 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
18949 break;
18950
18951 case 1:
18952 iobj_6->JointInternal.PositionNumber = 1.0;
18953 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
18954 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
18955 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
18956 break;
18957
18958 default:
18959 iobj_6->JointInternal.PositionNumber = 0.0;
18960 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
18961 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
18962 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
18963 break;
18964 }
18965
18966 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18967 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
18968 }
18969
18970 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
18971 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
18972 }
18973
18974 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
18975 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
18976 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
18977 obj->Bodies[7] = iobj_6;
18978 obj->NumBodies = 8.0;
18979 obj->PositionNumber = 6.0;
18980 obj_0 = &obj->Base;
18981 b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];
18982 obj->Base.NameInternal->size[0] = 1;
18983 obj->Base.NameInternal->size[1] = 5;
18984 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, b_kstr);
18985 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
18986 obj->Base.NameInternal->data[b_kstr] = tmp_8[b_kstr];
18987 }
18988
18989 b_kstr = obj->Base.JointInternal.Type->size[0] * obj->
18990 Base.JointInternal.Type->size[1];
18991 obj->Base.JointInternal.Type->size[0] = 1;
18992 obj->Base.JointInternal.Type->size[1] = 5;
18993 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.Type, b_kstr);
18994 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
18995 obj_0->JointInternal.Type->data[b_kstr] = tmp_7[b_kstr];
18996 }
18997
18998 b_kstr = switch_expression->size[0] * switch_expression->size[1];
18999 switch_expression->size[0] = 1;
19000 switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];
19001 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
19002 loop_ub = obj->Base.JointInternal.Type->size[0] * obj->
19003 Base.JointInternal.Type->size[1] - 1;
19004 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
19005 switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];
19006 }
19007
19008 b_bool = false;
19009 if (switch_expression->size[1] == 8) {
19010 b_kstr = 1;
19011 do {
19012 exitg1 = 0;
19013 if (b_kstr - 1 < 8) {
19014 loop_ub = b_kstr - 1;
19015 if (switch_expression->data[loop_ub] != b[loop_ub]) {
19016 exitg1 = 1;
19017 } else {
19018 b_kstr++;
19019 }
19020 } else {
19021 b_bool = true;
19022 exitg1 = 1;
19023 }
19024 } while (exitg1 == 0);
19025 }
19026
19027 if (b_bool) {
19028 b_kstr = 0;
19029 } else {
19030 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
19031 b_0[b_kstr] = tmp_1[b_kstr];
19032 }
19033
19034 b_bool = false;
19035 if (switch_expression->size[1] == 9) {
19036 b_kstr = 1;
19037 do {
19038 exitg1 = 0;
19039 if (b_kstr - 1 < 9) {
19040 loop_ub = b_kstr - 1;
19041 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
19042 exitg1 = 1;
19043 } else {
19044 b_kstr++;
19045 }
19046 } else {
19047 b_bool = true;
19048 exitg1 = 1;
19049 }
19050 } while (exitg1 == 0);
19051 }
19052
19053 if (b_bool) {
19054 b_kstr = 1;
19055 } else {
19056 b_kstr = -1;
19057 }
19058 }
19059
19060 cartesian_waypoi_emxFree_char_T(&switch_expression);
19061 switch (b_kstr) {
19062 case 0:
19063 obj->Base.JointInternal.PositionNumber = 1.0;
19064 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
19065 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
19066 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
19067 break;
19068
19069 case 1:
19070 obj->Base.JointInternal.PositionNumber = 1.0;
19071 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
19072 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
19073 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
19074 break;
19075
19076 default:
19077 obj->Base.JointInternal.PositionNumber = 0.0;
19078 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
19079 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
19080 obj->Base.JointInternal.JointAxisInternal[2] = 0.0;
19081 break;
19082 }
19083
19084 return b_obj;
19085}
19086
19087// Model step function
19088void cartesian_waypoints_planner_step(void)
19089{
19090 robotics_slmanip_internal_b_e_T *obj;
19091 emxArray_f_cell_wrap_cartesia_T *obj_e;
19092 boolean_T exitg1;
19093
19094 // MATLABSystem: '<S11>/Get Parameter'
19095 ParamGet_cartesian_waypoints_planner_346.get_parameter
19096 (&cartesian_waypoints_planner_B.value_h);
19097
19098 // MATLABSystem: '<S15>/Get Parameter'
19099 ParamGet_cartesian_waypoints_planner_288.get_parameter
19100 (&cartesian_waypoints_planner_B.c_n);
19101
19102 // MATLABSystem: '<S15>/Get Parameter1'
19103 ParamGet_cartesian_waypoints_planner_289.get_parameter
19104 (&cartesian_waypoints_planner_B.value);
19105
19106 // MATLABSystem: '<S15>/Get Parameter2'
19107 ParamGet_cartesian_waypoints_planner_290.get_parameter
19108 (&cartesian_waypoints_planner_B.value_n);
19109
19110 // MATLABSystem: '<S15>/Get Parameter3'
19111 ParamGet_cartesian_waypoints_planner_291.get_parameter
19112 (&cartesian_waypoints_planner_B.value_k);
19113
19114 // MATLABSystem: '<S15>/Get Parameter4'
19115 ParamGet_cartesian_waypoints_planner_292.get_parameter
19116 (&cartesian_waypoints_planner_B.value_n3);
19117
19118 // MATLABSystem: '<S15>/Get Parameter5'
19119 ParamGet_cartesian_waypoints_planner_293.get_parameter
19120 (&cartesian_waypoints_planner_B.value_o);
19121
19122 // MATLABSystem: '<S15>/Get Parameter6'
19123 ParamGet_cartesian_waypoints_planner_294.get_parameter
19124 (&cartesian_waypoints_planner_B.value_g);
19125
19126 // MATLABSystem: '<S16>/Get Parameter'
19127 ParamGet_cartesian_waypoints_planner_300.get_parameter
19128 (&cartesian_waypoints_planner_B.value_c);
19129
19130 // MATLABSystem: '<S16>/Get Parameter1'
19131 ParamGet_cartesian_waypoints_planner_301.get_parameter
19132 (&cartesian_waypoints_planner_B.value_cj);
19133
19134 // MATLABSystem: '<S16>/Get Parameter2'
19135 ParamGet_cartesian_waypoints_planner_302.get_parameter
19136 (&cartesian_waypoints_planner_B.value_m);
19137
19138 // MATLABSystem: '<S16>/Get Parameter3'
19139 ParamGet_cartesian_waypoints_planner_303.get_parameter
19140 (&cartesian_waypoints_planner_B.theta0);
19141
19142 // MATLABSystem: '<S16>/Get Parameter4'
19143 ParamGet_cartesian_waypoints_planner_304.get_parameter
19144 (&cartesian_waypoints_planner_B.value_j);
19145
19146 // MATLABSystem: '<S16>/Get Parameter5'
19147 ParamGet_cartesian_waypoints_planner_305.get_parameter
19148 (&cartesian_waypoints_planner_B.dp);
19149
19150 // MATLABSystem: '<S16>/Get Parameter6'
19151 ParamGet_cartesian_waypoints_planner_306.get_parameter
19152 (&cartesian_waypoints_planner_B.d_n);
19153
19154 // MATLAB Function: '<Root>/MATLAB Function' incorporates:
19155 // Delay: '<Root>/Delay3'
19156 // MATLABSystem: '<S11>/Get Parameter'
19157 // MATLABSystem: '<S15>/Get Parameter'
19158 // MATLABSystem: '<S15>/Get Parameter1'
19159 // MATLABSystem: '<S15>/Get Parameter2'
19160 // MATLABSystem: '<S15>/Get Parameter3'
19161 // MATLABSystem: '<S15>/Get Parameter4'
19162 // MATLABSystem: '<S15>/Get Parameter5'
19163 // MATLABSystem: '<S15>/Get Parameter6'
19164 // MATLABSystem: '<S16>/Get Parameter'
19165 // MATLABSystem: '<S16>/Get Parameter1'
19166 // MATLABSystem: '<S16>/Get Parameter2'
19167 // MATLABSystem: '<S16>/Get Parameter3'
19168 // MATLABSystem: '<S16>/Get Parameter4'
19169 // MATLABSystem: '<S16>/Get Parameter5'
19170 // MATLABSystem: '<S16>/Get Parameter6'
19171
19172 if (cartesian_waypoints_planner_B.value_h > 20) {
19173 cartesian_waypoints_planner_B.value_h = 20;
19174 }
19175
19176 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k = 0.0;
19177 cartesian_waypoints_planner_B.b_n_p = 0.0;
19178 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m = 0.0;
19179 cartesian_waypoints_planner_B.quat_fin_c = 0.0;
19180 cartesian_waypoints_planner_B.vec[0] = 0.0;
19181 cartesian_waypoints_planner_B.vec[1] = 0.0;
19182 cartesian_waypoints_planner_B.vec[2] = 0.0;
19183 if (cartesian_waypoints_planner_DW.Delay3_DSTATE <= 1.0) {
19184 cartesian_waypoints_planner_B.vec[0] = cartesian_waypoints_planner_B.c_n;
19185 cartesian_waypoints_planner_B.vec[1] = cartesian_waypoints_planner_B.value;
19186 cartesian_waypoints_planner_B.vec[2] = cartesian_waypoints_planner_B.value_n;
19187 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k =
19188 cartesian_waypoints_planner_B.value_k;
19189 cartesian_waypoints_planner_B.b_n_p = cartesian_waypoints_planner_B.value_n3;
19190 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m =
19191 cartesian_waypoints_planner_B.value_o;
19192 cartesian_waypoints_planner_B.quat_fin_c =
19193 cartesian_waypoints_planner_B.value_g;
19194 } else if (cartesian_waypoints_planner_DW.Delay3_DSTATE <
19195 cartesian_waypoints_planner_B.value_h) {
19196 cartesian_waypoints_planner_B.vec[0] =
19197 (cartesian_waypoints_planner_B.value_c - cartesian_waypoints_planner_B.c_n)
19198 / static_cast<real_T>(cartesian_waypoints_planner_B.value_h) *
19199 cartesian_waypoints_planner_DW.Delay3_DSTATE +
19200 cartesian_waypoints_planner_B.c_n;
19201 cartesian_waypoints_planner_B.vec[1] =
19202 (cartesian_waypoints_planner_B.value_cj -
19203 cartesian_waypoints_planner_B.value) / static_cast<real_T>
19204 (cartesian_waypoints_planner_B.value_h) *
19205 cartesian_waypoints_planner_DW.Delay3_DSTATE +
19206 cartesian_waypoints_planner_B.value;
19207 cartesian_waypoints_planner_B.vec[2] =
19208 (cartesian_waypoints_planner_B.value_m -
19209 cartesian_waypoints_planner_B.value_n) / static_cast<real_T>
19210 (cartesian_waypoints_planner_B.value_h) *
19211 cartesian_waypoints_planner_DW.Delay3_DSTATE +
19212 cartesian_waypoints_planner_B.value_n;
19213 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k =
19214 cartesian_waypoints_planner_DW.Delay3_DSTATE / static_cast<real_T>
19215 (cartesian_waypoints_planner_B.value_h);
19216 cartesian_waypoints_planner_B.b_n_p = sqrt
19217 (((cartesian_waypoints_planner_B.value_k *
19218 cartesian_waypoints_planner_B.value_k +
19219 cartesian_waypoints_planner_B.value_n3 *
19220 cartesian_waypoints_planner_B.value_n3) +
19221 cartesian_waypoints_planner_B.value_o *
19222 cartesian_waypoints_planner_B.value_o) +
19223 cartesian_waypoints_planner_B.value_g *
19224 cartesian_waypoints_planner_B.value_g);
19225 cartesian_waypoints_planner_B.value_k /= cartesian_waypoints_planner_B.b_n_p;
19226 cartesian_waypoints_planner_B.value_n3 /=
19227 cartesian_waypoints_planner_B.b_n_p;
19228 cartesian_waypoints_planner_B.value_o /= cartesian_waypoints_planner_B.b_n_p;
19229 cartesian_waypoints_planner_B.value_g /= cartesian_waypoints_planner_B.b_n_p;
19230 cartesian_waypoints_planner_B.c_n = sqrt
19231 (((cartesian_waypoints_planner_B.theta0 *
19232 cartesian_waypoints_planner_B.theta0 +
19233 cartesian_waypoints_planner_B.value_j *
19234 cartesian_waypoints_planner_B.value_j) +
19235 cartesian_waypoints_planner_B.dp * cartesian_waypoints_planner_B.dp) +
19236 cartesian_waypoints_planner_B.d_n * cartesian_waypoints_planner_B.d_n);
19237 cartesian_waypoints_planner_B.b_n_p = cartesian_waypoints_planner_B.theta0 /
19238 cartesian_waypoints_planner_B.c_n;
19239 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m =
19240 cartesian_waypoints_planner_B.value_j / cartesian_waypoints_planner_B.c_n;
19241 cartesian_waypoints_planner_B.quat_fin_c = cartesian_waypoints_planner_B.dp /
19242 cartesian_waypoints_planner_B.c_n;
19243 cartesian_waypoints_planner_B.d_n /= cartesian_waypoints_planner_B.c_n;
19244 cartesian_waypoints_planner_B.dp = ((cartesian_waypoints_planner_B.value_k *
19245 cartesian_waypoints_planner_B.b_n_p +
19246 cartesian_waypoints_planner_B.value_n3 *
19247 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m) +
19248 cartesian_waypoints_planner_B.value_o *
19249 cartesian_waypoints_planner_B.quat_fin_c) +
19250 cartesian_waypoints_planner_B.value_g * cartesian_waypoints_planner_B.d_n;
19251 if (cartesian_waypoints_planner_B.dp < 0.0) {
19252 car_quaternioncg_parenReference(cartesian_waypoints_planner_B.b_n_p,
19253 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m,
19254 cartesian_waypoints_planner_B.quat_fin_c,
19255 cartesian_waypoints_planner_B.d_n, &cartesian_waypoints_planner_B.b_n_p,
19256 &cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m,
19257 &cartesian_waypoints_planner_B.quat_fin_c,
19258 &cartesian_waypoints_planner_B.d_n);
19259 cartesian_waypoints_planner_B.b_n_p = -cartesian_waypoints_planner_B.b_n_p;
19260 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m =
19261 -cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m;
19262 cartesian_waypoints_planner_B.quat_fin_c =
19263 -cartesian_waypoints_planner_B.quat_fin_c;
19264 cartesian_waypoints_planner_B.d_n = -cartesian_waypoints_planner_B.d_n;
19265 cartesian_waypoints_planner_B.dp = -cartesian_waypoints_planner_B.dp;
19266 }
19267
19268 if (cartesian_waypoints_planner_B.dp > 1.0) {
19269 cartesian_waypoints_planner_B.dp = 1.0;
19270 }
19271
19272 cartesian_waypoints_planner_B.theta0 = acos(cartesian_waypoints_planner_B.dp);
19273 cartesian_waypoints_planner_B.dp = 1.0 / sin
19274 (cartesian_waypoints_planner_B.theta0);
19275 cartesian_waypoints_planner_B.value_j = sin((1.0 -
19276 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k) *
19277 cartesian_waypoints_planner_B.theta0);
19278 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k = sin
19279 (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k *
19280 cartesian_waypoints_planner_B.theta0);
19281 cartesian_waypoints_planner_B.b_n_p *=
19282 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k;
19283 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m *=
19284 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k;
19285 cartesian_waypoints_planner_B.quat_fin_c *=
19286 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k;
19287 cartesian_waypoints_planner_B.d_n *=
19288 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k;
19289 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k =
19290 (cartesian_waypoints_planner_B.value_j *
19291 cartesian_waypoints_planner_B.value_k +
19292 cartesian_waypoints_planner_B.b_n_p) * cartesian_waypoints_planner_B.dp;
19293 cartesian_waypoints_planner_B.b_n_p = (cartesian_waypoints_planner_B.value_j
19294 * cartesian_waypoints_planner_B.value_n3 +
19295 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m) *
19296 cartesian_waypoints_planner_B.dp;
19297 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m =
19298 (cartesian_waypoints_planner_B.value_j *
19299 cartesian_waypoints_planner_B.value_o +
19300 cartesian_waypoints_planner_B.quat_fin_c) *
19301 cartesian_waypoints_planner_B.dp;
19302 cartesian_waypoints_planner_B.quat_fin_c =
19303 (cartesian_waypoints_planner_B.value_j *
19304 cartesian_waypoints_planner_B.value_g + cartesian_waypoints_planner_B.d_n)
19305 * cartesian_waypoints_planner_B.dp;
19306 if (rtIsInf(cartesian_waypoints_planner_B.dp)) {
19307 car_quaternioncg_parenReference(cartesian_waypoints_planner_B.value_k,
19308 cartesian_waypoints_planner_B.value_n3,
19309 cartesian_waypoints_planner_B.value_o,
19310 cartesian_waypoints_planner_B.value_g,
19311 &cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k,
19312 &cartesian_waypoints_planner_B.b_n_p,
19313 &cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m,
19314 &cartesian_waypoints_planner_B.quat_fin_c);
19315 }
19316
19317 cartesian_waypoints_planner_B.d_n = sqrt
19318 (((cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k *
19319 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k +
19320 cartesian_waypoints_planner_B.b_n_p *
19321 cartesian_waypoints_planner_B.b_n_p) +
19322 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m *
19323 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m) +
19324 cartesian_waypoints_planner_B.quat_fin_c *
19325 cartesian_waypoints_planner_B.quat_fin_c);
19326 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k /=
19327 cartesian_waypoints_planner_B.d_n;
19328 cartesian_waypoints_planner_B.b_n_p /= cartesian_waypoints_planner_B.d_n;
19329 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m /=
19330 cartesian_waypoints_planner_B.d_n;
19331 cartesian_waypoints_planner_B.quat_fin_c /=
19332 cartesian_waypoints_planner_B.d_n;
19333 } else {
19334 if (cartesian_waypoints_planner_DW.Delay3_DSTATE >=
19335 cartesian_waypoints_planner_B.value_h) {
19336 cartesian_waypoints_planner_B.vec[0] =
19337 cartesian_waypoints_planner_B.value_c;
19338 cartesian_waypoints_planner_B.vec[1] =
19339 cartesian_waypoints_planner_B.value_cj;
19340 cartesian_waypoints_planner_B.vec[2] =
19341 cartesian_waypoints_planner_B.value_m;
19342 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k =
19343 cartesian_waypoints_planner_B.theta0;
19344 cartesian_waypoints_planner_B.b_n_p =
19345 cartesian_waypoints_planner_B.value_j;
19346 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m =
19347 cartesian_waypoints_planner_B.dp;
19348 cartesian_waypoints_planner_B.quat_fin_c =
19349 cartesian_waypoints_planner_B.d_n;
19350 }
19351 }
19352
19353 cartesian_waypoints_planner_DW.Delay3_DSTATE++;
19354
19355 // MATLABSystem: '<Root>/Coordinate Transformation Conversion3' incorporates:
19356 // MATLAB Function: '<Root>/MATLAB Function'
19357
19358 cartesian_waypoints_planner_B.d_n = 1.0 / sqrt
19359 (((cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k *
19360 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k +
19361 cartesian_waypoints_planner_B.b_n_p * cartesian_waypoints_planner_B.b_n_p)
19362 + cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m *
19363 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m) +
19364 cartesian_waypoints_planner_B.quat_fin_c *
19365 cartesian_waypoints_planner_B.quat_fin_c);
19366 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k *=
19367 cartesian_waypoints_planner_B.d_n;
19368 cartesian_waypoints_planner_B.b_n_p *= cartesian_waypoints_planner_B.d_n;
19369 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m *=
19370 cartesian_waypoints_planner_B.d_n;
19371 cartesian_waypoints_planner_B.d_n *= cartesian_waypoints_planner_B.quat_fin_c;
19372 cartesian_waypoints_planner_B.quat_fin_c = cartesian_waypoints_planner_B.d_n *
19373 cartesian_waypoints_planner_B.d_n;
19374 cartesian_waypoints_planner_B.dp =
19375 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m *
19376 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m;
19377 cartesian_waypoints_planner_B.tempR[0] = 1.0 -
19378 (cartesian_waypoints_planner_B.dp + cartesian_waypoints_planner_B.quat_fin_c)
19379 * 2.0;
19380 cartesian_waypoints_planner_B.value_k = cartesian_waypoints_planner_B.b_n_p *
19381 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m;
19382 cartesian_waypoints_planner_B.value_j =
19383 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k *
19384 cartesian_waypoints_planner_B.d_n;
19385 cartesian_waypoints_planner_B.tempR[1] =
19386 (cartesian_waypoints_planner_B.value_k -
19387 cartesian_waypoints_planner_B.value_j) * 2.0;
19388 cartesian_waypoints_planner_B.value_n3 = cartesian_waypoints_planner_B.b_n_p *
19389 cartesian_waypoints_planner_B.d_n;
19390 cartesian_waypoints_planner_B.theta0 =
19391 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k *
19392 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m;
19393 cartesian_waypoints_planner_B.tempR[2] =
19394 (cartesian_waypoints_planner_B.value_n3 +
19395 cartesian_waypoints_planner_B.theta0) * 2.0;
19396 cartesian_waypoints_planner_B.tempR[3] =
19397 (cartesian_waypoints_planner_B.value_k +
19398 cartesian_waypoints_planner_B.value_j) * 2.0;
19399 cartesian_waypoints_planner_B.value_k = cartesian_waypoints_planner_B.b_n_p *
19400 cartesian_waypoints_planner_B.b_n_p;
19401 cartesian_waypoints_planner_B.tempR[4] = 1.0 -
19402 (cartesian_waypoints_planner_B.value_k +
19403 cartesian_waypoints_planner_B.quat_fin_c) * 2.0;
19404 cartesian_waypoints_planner_B.quat_fin_c =
19405 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m *
19406 cartesian_waypoints_planner_B.d_n;
19407 cartesian_waypoints_planner_B.value_j =
19408 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k *
19409 cartesian_waypoints_planner_B.b_n_p;
19410 cartesian_waypoints_planner_B.tempR[5] =
19411 (cartesian_waypoints_planner_B.quat_fin_c -
19412 cartesian_waypoints_planner_B.value_j) * 2.0;
19413 cartesian_waypoints_planner_B.tempR[6] =
19414 (cartesian_waypoints_planner_B.value_n3 -
19415 cartesian_waypoints_planner_B.theta0) * 2.0;
19416 cartesian_waypoints_planner_B.tempR[7] =
19417 (cartesian_waypoints_planner_B.quat_fin_c +
19418 cartesian_waypoints_planner_B.value_j) * 2.0;
19419 cartesian_waypoints_planner_B.tempR[8] = 1.0 -
19420 (cartesian_waypoints_planner_B.value_k + cartesian_waypoints_planner_B.dp) *
19421 2.0;
19422 for (cartesian_waypoints_planner_B.value_h = 0;
19423 cartesian_waypoints_planner_B.value_h < 3;
19424 cartesian_waypoints_planner_B.value_h++) {
19425 cartesian_waypoints_planner_B.subsa_idx_1 =
19426 cartesian_waypoints_planner_B.value_h + 1;
19427 cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1 -
19428 1] = cartesian_waypoints_planner_B.tempR
19429 [(cartesian_waypoints_planner_B.subsa_idx_1 - 1) * 3];
19430 cartesian_waypoints_planner_B.subsa_idx_1 =
19431 cartesian_waypoints_planner_B.value_h + 1;
19432 cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1 +
19433 2] = cartesian_waypoints_planner_B.tempR
19434 [(cartesian_waypoints_planner_B.subsa_idx_1 - 1) * 3 + 1];
19435 cartesian_waypoints_planner_B.subsa_idx_1 =
19436 cartesian_waypoints_planner_B.value_h + 1;
19437 cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1 +
19438 5] = cartesian_waypoints_planner_B.tempR
19439 [(cartesian_waypoints_planner_B.subsa_idx_1 - 1) * 3 + 2];
19440 }
19441
19442 memset(&cartesian_waypoints_planner_B.out[0], 0, sizeof(real_T) << 4U);
19443 cartesian_waypoints_planner_B.out[15] = 1.0;
19444 for (cartesian_waypoints_planner_B.value_h = 0;
19445 cartesian_waypoints_planner_B.value_h < 3;
19446 cartesian_waypoints_planner_B.value_h++) {
19447 cartesian_waypoints_planner_B.subsa_idx_1 =
19448 cartesian_waypoints_planner_B.value_h << 2;
19449 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.subsa_idx_1]
19450 = cartesian_waypoints_planner_B.R[3 *
19451 cartesian_waypoints_planner_B.value_h];
19452 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.subsa_idx_1
19453 + 1] = cartesian_waypoints_planner_B.R[3 *
19454 cartesian_waypoints_planner_B.value_h + 1];
19455 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.subsa_idx_1
19456 + 2] = cartesian_waypoints_planner_B.R[3 *
19457 cartesian_waypoints_planner_B.value_h + 2];
19458 cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.value_h + 12]
19459 = cartesian_waypoints_planner_B.vec[cartesian_waypoints_planner_B.value_h];
19460 }
19461
19462 // MATLABSystem: '<S3>/MATLAB System' incorporates:
19463 // Delay: '<Root>/Delay'
19464 // MATLABSystem: '<Root>/Coordinate Transformation Conversion3'
19465 // MATLABSystem: '<S12>/Get Parameter'
19466 // MATLABSystem: '<S12>/Get Parameter1'
19467 // MATLABSystem: '<S12>/Get Parameter2'
19468 // MATLABSystem: '<S12>/Get Parameter3'
19469 // MATLABSystem: '<S12>/Get Parameter4'
19470 // MATLABSystem: '<S12>/Get Parameter5'
19471
19472 ParamGet_cartesian_waypoints_planner_312.get_parameter
19473 (&cartesian_waypoints_planner_B.q[0]);
19474 ParamGet_cartesian_waypoints_planner_313.get_parameter
19475 (&cartesian_waypoints_planner_B.q[1]);
19476 ParamGet_cartesian_waypoints_planner_314.get_parameter
19477 (&cartesian_waypoints_planner_B.q[2]);
19478 ParamGet_cartesian_waypoints_planner_315.get_parameter
19479 (&cartesian_waypoints_planner_B.q[3]);
19480 ParamGet_cartesian_waypoints_planner_316.get_parameter
19481 (&cartesian_waypoints_planner_B.q[4]);
19482 ParamGet_cartesian_waypoints_planner_317.get_parameter
19483 (&cartesian_waypoints_planner_B.q[5]);
19484 obj = &cartesian_waypoints_planner_DW.obj;
19485 if (cartesian_waypoints_planner_DW.obj.IKInternal.isInitialized != 1) {
19486 cartesian_waypoints_planner_DW.obj.IKInternal.isSetupComplete = false;
19487 cartesian_waypoints_planner_DW.obj.IKInternal.isInitialized = 1;
19488 car_inverseKinematics_setupImpl
19489 (&cartesian_waypoints_planner_DW.obj.IKInternal,
19490 &cartesian_waypoints_planner_DW.gobj_85);
19491 obj->IKInternal.isSetupComplete = true;
19492 }
19493
19494 cart_inverseKinematics_stepImpl(&obj->IKInternal,
19495 cartesian_waypoints_planner_B.out, cartesian_waypoints_planner_B.q,
19496 cartesian_waypoints_planner_DW.Delay_DSTATE,
19497 cartesian_waypoints_planner_B.b_varargout_1);
19498
19499 // MATLAB Function: '<Root>/MATLAB Function2' incorporates:
19500 // Delay: '<Root>/Delay1'
19501 // Delay: '<Root>/Delay2'
19502 // MATLABSystem: '<S3>/MATLAB System'
19503
19504 if ((cartesian_waypoints_planner_DW.Delay1_DSTATE >= 1.0) &&
19505 (cartesian_waypoints_planner_DW.Delay1_DSTATE <= 20.0)) {
19506 cartesian_waypoints_planner_B.value_h = static_cast<int32_T>
19507 (cartesian_waypoints_planner_DW.Delay1_DSTATE);
19508 for (cartesian_waypoints_planner_B.subsa_idx_1 = 0;
19509 cartesian_waypoints_planner_B.subsa_idx_1 < 6;
19510 cartesian_waypoints_planner_B.subsa_idx_1++) {
19511 cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.subsa_idx_1
19512 + 6 * (cartesian_waypoints_planner_B.value_h - 1)] =
19513 cartesian_waypoints_planner_B.b_varargout_1[cartesian_waypoints_planner_B.subsa_idx_1];
19514 }
19515 }
19516
19517 cartesian_w_emxInit_f_cell_wrap(&obj_e, 2);
19518
19519 // MATLAB Function: '<Root>/MATLAB Function2' incorporates:
19520 // Delay: '<Root>/Delay1'
19521
19522 cartesian_waypoints_planner_DW.Delay1_DSTATE++;
19523
19524 // MATLABSystem: '<S2>/MATLAB System' incorporates:
19525 // MATLABSystem: '<S3>/MATLAB System'
19526
19527 RigidBodyTree_forwardKinematics
19528 (&cartesian_waypoints_planner_DW.obj_e.TreeInternal,
19529 cartesian_waypoints_planner_B.b_varargout_1, obj_e);
19530
19531 // Outputs for Atomic SubSystem: '<Root>/Subscribe1'
19532 // MATLABSystem: '<S10>/SourceBlock'
19533 cartesian_waypoints_planner_B.b_varargout_1_b =
19534 Sub_cartesian_waypoints_planner_384.getLatestMessage
19535 (&cartesian_waypoints_planner_B.b_varargout_2);
19536
19537 // End of Outputs for SubSystem: '<Root>/Subscribe1'
19538 cartesian_w_emxFree_f_cell_wrap(&obj_e);
19539
19540 // Outputs for Atomic SubSystem: '<Root>/Subscribe1'
19541 // Outputs for Enabled SubSystem: '<S10>/Enabled Subsystem' incorporates:
19542 // EnablePort: '<S14>/Enable'
19543
19544 // MATLABSystem: '<S10>/SourceBlock' incorporates:
19545 // Inport: '<S14>/In1'
19546
19547 if (cartesian_waypoints_planner_B.b_varargout_1_b) {
19548 cartesian_waypoints_planner_B.In1 =
19549 cartesian_waypoints_planner_B.b_varargout_2;
19550 }
19551
19552 // End of Outputs for SubSystem: '<S10>/Enabled Subsystem'
19553 // End of Outputs for SubSystem: '<Root>/Subscribe1'
19554
19555 // MATLABSystem: '<Root>/Get Parameter'
19556 ParamGet_cartesian_waypoints_planner_378.get_parameter
19557 (&cartesian_waypoints_planner_B.c_n);
19558
19559 // MATLAB Function: '<Root>/MATLAB Function3' incorporates:
19560 // MATLABSystem: '<Root>/Get Parameter'
19561
19562 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k =
19563 (cartesian_waypoints_planner_B.In1.Clock_.Nsec / 1.0E+9 +
19564 cartesian_waypoints_planner_B.In1.Clock_.Sec) -
19565 cartesian_waypoints_planner_B.c_n;
19566 if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k < 0.0) {
19567 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k = 0.0;
19568 }
19569
19570 // End of MATLAB Function: '<Root>/MATLAB Function3'
19571
19572 // MATLABSystem: '<Root>/Get Parameter1'
19573 ParamGet_cartesian_waypoints_planner_379.get_parameter
19574 (&cartesian_waypoints_planner_B.c_n);
19575
19576 // MATLABSystem: '<Root>/Get Parameter2'
19577 ParamGet_cartesian_waypoints_planner_380.get_parameter
19578 (&cartesian_waypoints_planner_B.value);
19579
19580 // MATLAB Function: '<Root>/MATLAB Function4' incorporates:
19581 // Delay: '<Root>/Delay2'
19582 // MATLABSystem: '<Root>/Get Parameter1'
19583 // MATLABSystem: '<Root>/Get Parameter2'
19584
19585 for (cartesian_waypoints_planner_B.value_h = 0;
19586 cartesian_waypoints_planner_B.value_h < 6;
19587 cartesian_waypoints_planner_B.value_h++) {
19588 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.value_h] =
19589 cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.value_h];
19590 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.value_h] =
19591 0.0;
19592 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.value_h] =
19593 0.0;
19594 cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.value_h]
19595 = cartesian_waypoints_planner_B.c_n;
19596 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.value_h] =
19597 cartesian_waypoints_planner_B.value;
19598 }
19599
19600 memset(&cartesian_waypoints_planner_B.t[0], 0, 342U * sizeof(real_T));
19601 for (cartesian_waypoints_planner_B.value_h = 0;
19602 cartesian_waypoints_planner_B.value_h < 6;
19603 cartesian_waypoints_planner_B.value_h++) {
19604 cartesian_waypoints_planner_B.b_n_p =
19605 cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.value_h]
19606 / cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.value_h];
19607 for (cartesian_waypoints_planner_B.subsa_idx_1 = 0;
19608 cartesian_waypoints_planner_B.subsa_idx_1 < 19;
19609 cartesian_waypoints_planner_B.subsa_idx_1++) {
19610 cartesian_waypoints_planner_B.dist_tmp = 6 *
19611 cartesian_waypoints_planner_B.subsa_idx_1 +
19612 cartesian_waypoints_planner_B.value_h;
19613 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m =
19614 cartesian_waypoints_planner_DW.Delay2_DSTATE
19615 [(cartesian_waypoints_planner_B.subsa_idx_1 + 1) * 6 +
19616 cartesian_waypoints_planner_B.value_h] -
19617 cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.dist_tmp];
19618 if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m < 0.0) {
19619 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]
19620 = -1.0;
19621 } else if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m >
19622 0.0) {
19623 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]
19624 = 1.0;
19625 } else if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m ==
19626 0.0) {
19627 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]
19628 = 0.0;
19629 } else {
19630 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]
19631 = (rtNaN);
19632 }
19633
19634 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m = fabs
19635 (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m);
19636 cartesian_waypoints_planner_B.d_n = cartesian_waypoints_planner_B.b_n_p *
19637 cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.value_h];
19638 if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m <
19639 cartesian_waypoints_planner_B.d_n) {
19640 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m = sqrt
19641 (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m /
19642 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.value_h]);
19643 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp +
19644 114] = 0.0;
19645 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp] =
19646 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m;
19647 cartesian_waypoints_planner_B.t[(cartesian_waypoints_planner_B.value_h +
19648 6 * cartesian_waypoints_planner_B.subsa_idx_1) + 228] =
19649 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m;
19650 cartesian_waypoints_planner_B.act_max_vel[cartesian_waypoints_planner_B.dist_tmp]
19651 = cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m *
19652 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.value_h];
19653 } else {
19654 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp] =
19655 cartesian_waypoints_planner_B.b_n_p;
19656 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp +
19657 228] = cartesian_waypoints_planner_B.b_n_p;
19658 cartesian_waypoints_planner_B.t[(cartesian_waypoints_planner_B.value_h +
19659 6 * cartesian_waypoints_planner_B.subsa_idx_1) + 114] =
19660 (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m -
19661 cartesian_waypoints_planner_B.d_n) /
19662 cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.value_h];
19663 cartesian_waypoints_planner_B.act_max_vel[cartesian_waypoints_planner_B.dist_tmp]
19664 =
19665 cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.value_h];
19666 }
19667 }
19668 }
19669
19670 for (cartesian_waypoints_planner_B.dist_tmp = 0;
19671 cartesian_waypoints_planner_B.dist_tmp < 6;
19672 cartesian_waypoints_planner_B.dist_tmp++) {
19673 cartesian_waypoints_planner_B.b_n_p =
19674 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__k;
19675 cartesian_waypoints_planner_B.b_j_f = 0;
19676 while ((cartesian_waypoints_planner_B.b_j_f < 19) &&
19677 (!(cartesian_waypoints_planner_B.b_n_p <= 0.0))) {
19678 cartesian_waypoints_planner_B.k_j = 0;
19679 exitg1 = false;
19680 while ((!exitg1) && (cartesian_waypoints_planner_B.k_j < 3)) {
19681 cartesian_waypoints_planner_B.value_h = 6 *
19682 cartesian_waypoints_planner_B.b_j_f +
19683 cartesian_waypoints_planner_B.dist_tmp;
19684 cartesian_waypoints_planner_B.subsa_idx_1 =
19685 cartesian_waypoints_planner_B.value_h + 114 *
19686 cartesian_waypoints_planner_B.k_j;
19687 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m =
19688 cartesian_waypoints_planner_B.b_n_p -
19689 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1];
19690 if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m <= 0.0)
19691 {
19692 switch (cartesian_waypoints_planner_B.k_j + 1) {
19693 case 1:
19694 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.dist_tmp]
19695 += 0.5 *
19696 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.dist_tmp]
19697 * (cartesian_waypoints_planner_B.b_n_p *
19698 cartesian_waypoints_planner_B.b_n_p) *
19699 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h];
19700 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.dist_tmp]
19701 =
19702 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h]
19703 * (cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.dist_tmp]
19704 * cartesian_waypoints_planner_B.b_n_p);
19705 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.dist_tmp]
19706 =
19707 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h]
19708 * cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.dist_tmp];
19709 cartesian_waypoints_planner_B.b_n_p =
19710 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m;
19711 break;
19712
19713 case 2:
19714 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.dist_tmp]
19715 +=
19716 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h]
19717 * (cartesian_waypoints_planner_B.act_max_vel[cartesian_waypoints_planner_B.dist_tmp]
19718 * cartesian_waypoints_planner_B.b_n_p);
19719 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.dist_tmp]
19720 =
19721 cartesian_waypoints_planner_B.act_max_vel[cartesian_waypoints_planner_B.dist_tmp]
19722 * cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp];
19723 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.dist_tmp]
19724 = 0.0;
19725 cartesian_waypoints_planner_B.b_n_p =
19726 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m;
19727 break;
19728
19729 default:
19730 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.dist_tmp]
19731 +=
19732 (cartesian_waypoints_planner_B.act_max_vel[cartesian_waypoints_planner_B.b_j_f]
19733 * cartesian_waypoints_planner_B.b_n_p - 0.5 *
19734 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.dist_tmp]
19735 * (cartesian_waypoints_planner_B.b_n_p *
19736 cartesian_waypoints_planner_B.b_n_p)) *
19737 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h];
19738 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.dist_tmp]
19739 =
19740 (cartesian_waypoints_planner_B.act_max_vel[cartesian_waypoints_planner_B.dist_tmp]
19741 - cartesian_waypoints_planner_B.b_n_p *
19742 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.dist_tmp])
19743 * cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h];
19744 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.dist_tmp]
19745 =
19746 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h]
19747 * -cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.dist_tmp];
19748 cartesian_waypoints_planner_B.b_n_p =
19749 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m;
19750 break;
19751 }
19752
19753 exitg1 = true;
19754 } else {
19755 cartesian_waypoints_planner_B.b_n_p =
19756 cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__m;
19757 switch (cartesian_waypoints_planner_B.k_j + 1) {
19758 case 1:
19759 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.dist_tmp]
19760 +=
19761 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1]
19762 * cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1]
19763 * (0.5 *
19764 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.dist_tmp])
19765 * cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h];
19766 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.dist_tmp]
19767 =
19768 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h]
19769 * cartesian_waypoints_planner_B.act_max_vel[cartesian_waypoints_planner_B.dist_tmp];
19770 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.dist_tmp]
19771 =
19772 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h]
19773 * cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.dist_tmp];
19774 break;
19775
19776 case 2:
19777 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.dist_tmp]
19778 +=
19779 cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1]
19780 * cartesian_waypoints_planner_B.act_max_vel[cartesian_waypoints_planner_B.dist_tmp]
19781 * cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h];
19782 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.dist_tmp]
19783 =
19784 cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h]
19785 * cartesian_waypoints_planner_B.act_max_vel[cartesian_waypoints_planner_B.dist_tmp];
19786 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.dist_tmp]
19787 = 0.0;
19788 break;
19789
19790 default:
19791 cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.dist_tmp]
19792 +=
19793 (cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1]
19794 * cartesian_waypoints_planner_B.act_max_vel[cartesian_waypoints_planner_B.dist_tmp]
19795 - cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1]
19796 * cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1]
19797 * (0.5 *
19798 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.dist_tmp]))
19799 * cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.value_h];
19800 cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.dist_tmp]
19801 = 0.0;
19802 cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.dist_tmp]
19803 = 0.0;
19804 break;
19805 }
19806
19807 cartesian_waypoints_planner_B.k_j++;
19808 }
19809 }
19810
19811 cartesian_waypoints_planner_B.b_j_f++;
19812 }
19813 }
19814
19815 // End of MATLAB Function: '<Root>/MATLAB Function4'
19816
19817 // MATLABSystem: '<S13>/Set Parameter' incorporates:
19818 // Delay: '<Root>/Delay2'
19819 // MATLAB Function: '<Root>/MATLAB Function2'
19820
19821 ParamSet_cartesian_waypoints_planner_364.set_parameter
19822 (cartesian_waypoints_planner_DW.Delay2_DSTATE[2]);
19823
19824 // MATLABSystem: '<S13>/Set Parameter1' incorporates:
19825 // Delay: '<Root>/Delay2'
19826 // MATLAB Function: '<Root>/MATLAB Function2'
19827
19828 ParamSet_cartesian_waypoints_planner_365.set_parameter
19829 (cartesian_waypoints_planner_DW.Delay2_DSTATE[3]);
19830
19831 // MATLABSystem: '<S13>/Set Parameter2' incorporates:
19832 // Delay: '<Root>/Delay2'
19833 // MATLAB Function: '<Root>/MATLAB Function2'
19834
19835 ParamSet_cartesian_waypoints_planner_366.set_parameter
19836 (cartesian_waypoints_planner_DW.Delay2_DSTATE[4]);
19837
19838 // MATLABSystem: '<S13>/Set Parameter3' incorporates:
19839 // Delay: '<Root>/Delay2'
19840 // MATLAB Function: '<Root>/MATLAB Function2'
19841
19842 ParamSet_cartesian_waypoints_planner_371.set_parameter
19843 (cartesian_waypoints_planner_DW.Delay2_DSTATE[5]);
19844
19845 // MATLABSystem: '<S13>/Set Parameter4' incorporates:
19846 // Delay: '<Root>/Delay2'
19847 // MATLAB Function: '<Root>/MATLAB Function2'
19848
19849 ParamSet_cartesian_waypoints_planner_372.set_parameter
19850 (cartesian_waypoints_planner_DW.Delay2_DSTATE[1]);
19851
19852 // MATLABSystem: '<S13>/Set Parameter5' incorporates:
19853 // Delay: '<Root>/Delay2'
19854 // MATLAB Function: '<Root>/MATLAB Function2'
19855
19856 ParamSet_cartesian_waypoints_planner_373.set_parameter
19857 (cartesian_waypoints_planner_DW.Delay2_DSTATE[0]);
19858
19859 // MATLAB Function: '<Root>/MATLAB Function1' incorporates:
19860 // BusAssignment: '<Root>/Bus Assignment'
19861 // Constant: '<S1>/Constant'
19862
19863 cartesian_waypoints_planner_B.msg =
19864 cartesian_waypoints_planner_P.Constant_Value;
19865
19866 // BusAssignment: '<Root>/Bus Assignment' incorporates:
19867 // MATLAB Function: '<Root>/MATLAB Function1'
19868
19869 cartesian_waypoints_planner_B.msg.TimeFromStart.Sec =
19870 cartesian_waypoints_planner_B.In1.Clock_.Sec;
19871 cartesian_waypoints_planner_B.msg.TimeFromStart.Nsec =
19872 cartesian_waypoints_planner_B.In1.Clock_.Nsec;
19873
19874 // MATLAB Function: '<Root>/MATLAB Function1'
19875 cartesian_waypoints_planner_B.msg.Velocities_SL_Info.CurrentLength = 6U;
19876 cartesian_waypoints_planner_B.msg.Positions_SL_Info.CurrentLength = 6U;
19877 cartesian_waypoints_planner_B.msg.Accelerations_SL_Info.CurrentLength = 6U;
19878 for (cartesian_waypoints_planner_B.value_h = 0;
19879 cartesian_waypoints_planner_B.value_h < 6;
19880 cartesian_waypoints_planner_B.value_h++) {
19881 cartesian_waypoints_planner_B.msg.Velocities[cartesian_waypoints_planner_B.value_h]
19882 = cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.value_h];
19883 cartesian_waypoints_planner_B.msg.Positions[cartesian_waypoints_planner_B.value_h]
19884 = cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.value_h];
19885 cartesian_waypoints_planner_B.msg.Accelerations[cartesian_waypoints_planner_B.value_h]
19886 = cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.value_h];
19887 }
19888
19889 // Outputs for Atomic SubSystem: '<Root>/Publish'
19890 // MATLABSystem: '<S9>/SinkBlock'
19891 Pub_cartesian_waypoints_planner_392.publish(&cartesian_waypoints_planner_B.msg);
19892
19893 // End of Outputs for SubSystem: '<Root>/Publish'
19894
19895 // Update for Delay: '<Root>/Delay' incorporates:
19896 // MATLABSystem: '<S3>/MATLAB System'
19897
19898 for (cartesian_waypoints_planner_B.value_h = 0;
19899 cartesian_waypoints_planner_B.value_h < 6;
19900 cartesian_waypoints_planner_B.value_h++) {
19901 cartesian_waypoints_planner_DW.Delay_DSTATE[cartesian_waypoints_planner_B.value_h]
19902 =
19903 cartesian_waypoints_planner_B.b_varargout_1[cartesian_waypoints_planner_B.value_h];
19904 }
19905
19906 // End of Update for Delay: '<Root>/Delay'
19907}
19908
19909// Model initialize function
19910void cartesian_waypoints_planner_initialize(void)
19911{
19912 // Registration code
19913
19914 // initialize non-finites
19915 rt_InitInfAndNaN(sizeof(real_T));
19916
19917 {
19918 robotics_slmanip_internal_b_e_T *obj;
19919 b_inverseKinematics_cartesian_T *obj_0;
19920 h_robotics_core_internal_Damp_T *obj_1;
19921 static const char_T tmp[6] = { '/', 'c', 'l', 'o', 'c', 'k' };
19922
19923 static const char_T tmp_0[17] = { '/', 'j', 'o', 'i', 'n', 't', '_', 't',
19924 'r', 'a', 'j', 'e', 'c', 't', 'o', 'r', 'y' };
19925
19926 static const char_T tmp_1[9] = { '/', 'n', '_', 'p', 'o', 'i', 'n', 't', 's'
19927 };
19928
19929 static const char_T tmp_2[5] = { '/', 'p', 'i', '_', 'x' };
19930
19931 static const char_T tmp_3[5] = { '/', 'p', 'i', '_', 'y' };
19932
19933 static const char_T tmp_4[5] = { '/', 'p', 'i', '_', 'z' };
19934
19935 static const char_T tmp_5[5] = { '/', 'o', 'i', '_', 'x' };
19936
19937 static const char_T tmp_6[5] = { '/', 'o', 'i', '_', 'y' };
19938
19939 static const char_T tmp_7[5] = { '/', 'o', 'i', '_', 'z' };
19940
19941 static const char_T tmp_8[5] = { '/', 'o', 'i', '_', 'w' };
19942
19943 static const char_T tmp_9[5] = { '/', 'p', 'f', '_', 'x' };
19944
19945 static const char_T tmp_a[5] = { '/', 'p', 'f', '_', 'y' };
19946
19947 static const char_T tmp_b[5] = { '/', 'p', 'f', '_', 'z' };
19948
19949 static const char_T tmp_c[5] = { '/', 'o', 'f', '_', 'x' };
19950
19951 static const char_T tmp_d[5] = { '/', 'o', 'f', '_', 'y' };
19952
19953 static const char_T tmp_e[5] = { '/', 'o', 'f', '_', 'z' };
19954
19955 static const char_T tmp_f[5] = { '/', 'o', 'f', '_', 'w' };
19956
19957 static const char_T tmp_g[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
19958 'o', 'x' };
19959
19960 static const char_T tmp_h[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
19961 'o', 'y' };
19962
19963 static const char_T tmp_i[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
19964 'o', 'z' };
19965
19966 static const char_T tmp_j[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
19967 'p', 'x' };
19968
19969 static const char_T tmp_k[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
19970 'p', 'y' };
19971
19972 static const char_T tmp_l[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
19973 'p', 'z' };
19974
19975 static const char_T tmp_m[12] = { '/', 's', 't', 'a', 'r', 't', '_', 'd',
19976 'e', 'l', 'a', 'y' };
19977
19978 static const char_T tmp_n[12] = { '/', 'm', 'a', 'x', '_', 'a', 'n', 'g',
19979 '_', 'v', 'e', 'l' };
19980
19981 static const char_T tmp_o[12] = { '/', 'm', 'a', 'x', '_', 'a', 'n', 'g',
19982 '_', 'a', 'c', 'c' };
19983
19984 static const char_T tmp_p[11] = { '/', 'q', '3', '_', 'i', 'n', 'i', 't',
19985 'i', 'a', 'l' };
19986
19987 static const char_T tmp_q[11] = { '/', 'q', '4', '_', 'i', 'n', 'i', 't',
19988 'i', 'a', 'l' };
19989
19990 static const char_T tmp_r[11] = { '/', 'q', '5', '_', 'i', 'n', 'i', 't',
19991 'i', 'a', 'l' };
19992
19993 static const char_T tmp_s[11] = { '/', 'q', '6', '_', 'i', 'n', 'i', 't',
19994 'i', 'a', 'l' };
19995
19996 static const char_T tmp_t[11] = { '/', 'q', '2', '_', 'i', 'n', 'i', 't',
19997 'i', 'a', 'l' };
19998
19999 static const char_T tmp_u[11] = { '/', 'q', '1', '_', 'i', 'n', 'i', 't',
20000 'i', 'a', 'l' };
20001
20002 // InitializeConditions for Delay: '<Root>/Delay2'
20003 memcpy(&cartesian_waypoints_planner_DW.Delay2_DSTATE[0],
20004 &cartesian_waypoints_planner_P.Delay2_InitialCondition[0], 120U *
20005 sizeof(real_T));
20006
20007 // InitializeConditions for Delay: '<Root>/Delay3'
20008 cartesian_waypoints_planner_DW.Delay3_DSTATE =
20009 cartesian_waypoints_planner_P.Delay3_InitialCondition;
20010
20011 // InitializeConditions for Delay: '<Root>/Delay1'
20012 cartesian_waypoints_planner_DW.Delay1_DSTATE =
20013 cartesian_waypoints_planner_P.Delay1_InitialCondition;
20014
20015 // SystemInitialize for Atomic SubSystem: '<Root>/Subscribe1'
20016 // SystemInitialize for Enabled SubSystem: '<S10>/Enabled Subsystem'
20017 // SystemInitialize for Outport: '<S14>/Out1'
20018 cartesian_waypoints_planner_B.In1 = cartesian_waypoints_planner_P.Out1_Y0;
20019
20020 // End of SystemInitialize for SubSystem: '<S10>/Enabled Subsystem'
20021
20022 // Start for MATLABSystem: '<S10>/SourceBlock'
20023 cartesian_waypoints_planner_DW.obj_pt.matlabCodegenIsDeleted = false;
20024 cartesian_waypoints_planner_DW.obj_pt.isInitialized = 1;
20025 for (cartesian_waypoints_planner_B.i_k = 0;
20026 cartesian_waypoints_planner_B.i_k < 6;
20027 cartesian_waypoints_planner_B.i_k++) {
20028 // InitializeConditions for Delay: '<Root>/Delay'
20029 cartesian_waypoints_planner_DW.Delay_DSTATE[cartesian_waypoints_planner_B.i_k]
20030 =
20031 cartesian_waypoints_planner_P.Delay_InitialCondition[cartesian_waypoints_planner_B.i_k];
20032
20033 // Start for MATLABSystem: '<S10>/SourceBlock'
20034 cartesian_waypoints_planner_B.cv5[cartesian_waypoints_planner_B.i_k] =
20035 tmp[cartesian_waypoints_planner_B.i_k];
20036 }
20037
20038 // Start for MATLABSystem: '<S10>/SourceBlock'
20039 cartesian_waypoints_planner_B.cv5[6] = '\x00';
20040 Sub_cartesian_waypoints_planner_384.createSubscriber
20041 (cartesian_waypoints_planner_B.cv5, 1);
20042 cartesian_waypoints_planner_DW.obj_pt.isSetupComplete = true;
20043
20044 // End of SystemInitialize for SubSystem: '<Root>/Subscribe1'
20045
20046 // SystemInitialize for Atomic SubSystem: '<Root>/Publish'
20047 // Start for MATLABSystem: '<S9>/SinkBlock'
20048 cartesian_waypoints_planner_DW.obj_b.matlabCodegenIsDeleted = false;
20049 cartesian_waypoints_planner_DW.obj_b.isInitialized = 1;
20050 for (cartesian_waypoints_planner_B.i_k = 0;
20051 cartesian_waypoints_planner_B.i_k < 17;
20052 cartesian_waypoints_planner_B.i_k++) {
20053 cartesian_waypoints_planner_B.cv[cartesian_waypoints_planner_B.i_k] =
20054 tmp_0[cartesian_waypoints_planner_B.i_k];
20055 }
20056
20057 cartesian_waypoints_planner_B.cv[17] = '\x00';
20058 Pub_cartesian_waypoints_planner_392.createPublisher
20059 (cartesian_waypoints_planner_B.cv, 1);
20060 cartesian_waypoints_planner_DW.obj_b.isSetupComplete = true;
20061
20062 // End of Start for MATLABSystem: '<S9>/SinkBlock'
20063 // End of SystemInitialize for SubSystem: '<Root>/Publish'
20064
20065 // Start for MATLABSystem: '<S11>/Get Parameter'
20066 cartesian_waypoints_planner_DW.obj_eq.matlabCodegenIsDeleted = false;
20067 cartesian_waypoints_planner_DW.obj_eq.isInitialized = 1;
20068 for (cartesian_waypoints_planner_B.i_k = 0;
20069 cartesian_waypoints_planner_B.i_k < 9;
20070 cartesian_waypoints_planner_B.i_k++) {
20071 cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_k] =
20072 tmp_1[cartesian_waypoints_planner_B.i_k];
20073 }
20074
20075 cartesian_waypoints_planner_B.cv4[9] = '\x00';
20076 ParamGet_cartesian_waypoints_planner_346.initialize
20077 (cartesian_waypoints_planner_B.cv4);
20078 ParamGet_cartesian_waypoints_planner_346.initialize_error_codes(0, 1, 2, 3);
20079 ParamGet_cartesian_waypoints_planner_346.set_initial_value(3);
20080 cartesian_waypoints_planner_DW.obj_eq.isSetupComplete = true;
20081
20082 // End of Start for MATLABSystem: '<S11>/Get Parameter'
20083
20084 // Start for MATLABSystem: '<S15>/Get Parameter'
20085 cartesian_waypoints_planner_DW.obj_i.matlabCodegenIsDeleted = false;
20086 cartesian_waypoints_planner_DW.obj_i.isInitialized = 1;
20087 for (cartesian_waypoints_planner_B.i_k = 0;
20088 cartesian_waypoints_planner_B.i_k < 5;
20089 cartesian_waypoints_planner_B.i_k++) {
20090 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20091 tmp_2[cartesian_waypoints_planner_B.i_k];
20092 }
20093
20094 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20095 ParamGet_cartesian_waypoints_planner_288.initialize
20096 (cartesian_waypoints_planner_B.cv6);
20097 ParamGet_cartesian_waypoints_planner_288.initialize_error_codes(0, 1, 2, 3);
20098 ParamGet_cartesian_waypoints_planner_288.set_initial_value(0.0);
20099 cartesian_waypoints_planner_DW.obj_i.isSetupComplete = true;
20100
20101 // End of Start for MATLABSystem: '<S15>/Get Parameter'
20102
20103 // Start for MATLABSystem: '<S15>/Get Parameter1'
20104 cartesian_waypoints_planner_DW.obj_db.matlabCodegenIsDeleted = false;
20105 cartesian_waypoints_planner_DW.obj_db.isInitialized = 1;
20106 for (cartesian_waypoints_planner_B.i_k = 0;
20107 cartesian_waypoints_planner_B.i_k < 5;
20108 cartesian_waypoints_planner_B.i_k++) {
20109 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20110 tmp_3[cartesian_waypoints_planner_B.i_k];
20111 }
20112
20113 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20114 ParamGet_cartesian_waypoints_planner_289.initialize
20115 (cartesian_waypoints_planner_B.cv6);
20116 ParamGet_cartesian_waypoints_planner_289.initialize_error_codes(0, 1, 2, 3);
20117 ParamGet_cartesian_waypoints_planner_289.set_initial_value(0.0);
20118 cartesian_waypoints_planner_DW.obj_db.isSetupComplete = true;
20119
20120 // End of Start for MATLABSystem: '<S15>/Get Parameter1'
20121
20122 // Start for MATLABSystem: '<S15>/Get Parameter2'
20123 cartesian_waypoints_planner_DW.obj_oq.matlabCodegenIsDeleted = false;
20124 cartesian_waypoints_planner_DW.obj_oq.isInitialized = 1;
20125 for (cartesian_waypoints_planner_B.i_k = 0;
20126 cartesian_waypoints_planner_B.i_k < 5;
20127 cartesian_waypoints_planner_B.i_k++) {
20128 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20129 tmp_4[cartesian_waypoints_planner_B.i_k];
20130 }
20131
20132 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20133 ParamGet_cartesian_waypoints_planner_290.initialize
20134 (cartesian_waypoints_planner_B.cv6);
20135 ParamGet_cartesian_waypoints_planner_290.initialize_error_codes(0, 1, 2, 3);
20136 ParamGet_cartesian_waypoints_planner_290.set_initial_value(0.99);
20137 cartesian_waypoints_planner_DW.obj_oq.isSetupComplete = true;
20138
20139 // End of Start for MATLABSystem: '<S15>/Get Parameter2'
20140
20141 // Start for MATLABSystem: '<S15>/Get Parameter3'
20142 cartesian_waypoints_planner_DW.obj_esa.matlabCodegenIsDeleted = false;
20143 cartesian_waypoints_planner_DW.obj_esa.isInitialized = 1;
20144 for (cartesian_waypoints_planner_B.i_k = 0;
20145 cartesian_waypoints_planner_B.i_k < 5;
20146 cartesian_waypoints_planner_B.i_k++) {
20147 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20148 tmp_5[cartesian_waypoints_planner_B.i_k];
20149 }
20150
20151 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20152 ParamGet_cartesian_waypoints_planner_291.initialize
20153 (cartesian_waypoints_planner_B.cv6);
20154 ParamGet_cartesian_waypoints_planner_291.initialize_error_codes(0, 1, 2, 3);
20155 ParamGet_cartesian_waypoints_planner_291.set_initial_value(0.0);
20156 cartesian_waypoints_planner_DW.obj_esa.isSetupComplete = true;
20157
20158 // End of Start for MATLABSystem: '<S15>/Get Parameter3'
20159
20160 // Start for MATLABSystem: '<S15>/Get Parameter4'
20161 cartesian_waypoints_planner_DW.obj_f4.matlabCodegenIsDeleted = false;
20162 cartesian_waypoints_planner_DW.obj_f4.isInitialized = 1;
20163 for (cartesian_waypoints_planner_B.i_k = 0;
20164 cartesian_waypoints_planner_B.i_k < 5;
20165 cartesian_waypoints_planner_B.i_k++) {
20166 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20167 tmp_6[cartesian_waypoints_planner_B.i_k];
20168 }
20169
20170 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20171 ParamGet_cartesian_waypoints_planner_292.initialize
20172 (cartesian_waypoints_planner_B.cv6);
20173 ParamGet_cartesian_waypoints_planner_292.initialize_error_codes(0, 1, 2, 3);
20174 ParamGet_cartesian_waypoints_planner_292.set_initial_value(0.0);
20175 cartesian_waypoints_planner_DW.obj_f4.isSetupComplete = true;
20176
20177 // End of Start for MATLABSystem: '<S15>/Get Parameter4'
20178
20179 // Start for MATLABSystem: '<S15>/Get Parameter5'
20180 cartesian_waypoints_planner_DW.obj_en.matlabCodegenIsDeleted = false;
20181 cartesian_waypoints_planner_DW.obj_en.isInitialized = 1;
20182 for (cartesian_waypoints_planner_B.i_k = 0;
20183 cartesian_waypoints_planner_B.i_k < 5;
20184 cartesian_waypoints_planner_B.i_k++) {
20185 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20186 tmp_7[cartesian_waypoints_planner_B.i_k];
20187 }
20188
20189 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20190 ParamGet_cartesian_waypoints_planner_293.initialize
20191 (cartesian_waypoints_planner_B.cv6);
20192 ParamGet_cartesian_waypoints_planner_293.initialize_error_codes(0, 1, 2, 3);
20193 ParamGet_cartesian_waypoints_planner_293.set_initial_value(0.0);
20194 cartesian_waypoints_planner_DW.obj_en.isSetupComplete = true;
20195
20196 // End of Start for MATLABSystem: '<S15>/Get Parameter5'
20197
20198 // Start for MATLABSystem: '<S15>/Get Parameter6'
20199 cartesian_waypoints_planner_DW.obj_l0.matlabCodegenIsDeleted = false;
20200 cartesian_waypoints_planner_DW.obj_l0.isInitialized = 1;
20201 for (cartesian_waypoints_planner_B.i_k = 0;
20202 cartesian_waypoints_planner_B.i_k < 5;
20203 cartesian_waypoints_planner_B.i_k++) {
20204 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20205 tmp_8[cartesian_waypoints_planner_B.i_k];
20206 }
20207
20208 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20209 ParamGet_cartesian_waypoints_planner_294.initialize
20210 (cartesian_waypoints_planner_B.cv6);
20211 ParamGet_cartesian_waypoints_planner_294.initialize_error_codes(0, 1, 2, 3);
20212 ParamGet_cartesian_waypoints_planner_294.set_initial_value(1.0);
20213 cartesian_waypoints_planner_DW.obj_l0.isSetupComplete = true;
20214
20215 // End of Start for MATLABSystem: '<S15>/Get Parameter6'
20216
20217 // Start for MATLABSystem: '<S16>/Get Parameter'
20218 cartesian_waypoints_planner_DW.obj_mb.matlabCodegenIsDeleted = false;
20219 cartesian_waypoints_planner_DW.obj_mb.isInitialized = 1;
20220 for (cartesian_waypoints_planner_B.i_k = 0;
20221 cartesian_waypoints_planner_B.i_k < 5;
20222 cartesian_waypoints_planner_B.i_k++) {
20223 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20224 tmp_9[cartesian_waypoints_planner_B.i_k];
20225 }
20226
20227 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20228 ParamGet_cartesian_waypoints_planner_300.initialize
20229 (cartesian_waypoints_planner_B.cv6);
20230 ParamGet_cartesian_waypoints_planner_300.initialize_error_codes(0, 1, 2, 3);
20231 ParamGet_cartesian_waypoints_planner_300.set_initial_value(0.3);
20232 cartesian_waypoints_planner_DW.obj_mb.isSetupComplete = true;
20233
20234 // End of Start for MATLABSystem: '<S16>/Get Parameter'
20235
20236 // Start for MATLABSystem: '<S16>/Get Parameter1'
20237 cartesian_waypoints_planner_DW.obj_c.matlabCodegenIsDeleted = false;
20238 cartesian_waypoints_planner_DW.obj_c.isInitialized = 1;
20239 for (cartesian_waypoints_planner_B.i_k = 0;
20240 cartesian_waypoints_planner_B.i_k < 5;
20241 cartesian_waypoints_planner_B.i_k++) {
20242 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20243 tmp_a[cartesian_waypoints_planner_B.i_k];
20244 }
20245
20246 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20247 ParamGet_cartesian_waypoints_planner_301.initialize
20248 (cartesian_waypoints_planner_B.cv6);
20249 ParamGet_cartesian_waypoints_planner_301.initialize_error_codes(0, 1, 2, 3);
20250 ParamGet_cartesian_waypoints_planner_301.set_initial_value(0.3);
20251 cartesian_waypoints_planner_DW.obj_c.isSetupComplete = true;
20252
20253 // End of Start for MATLABSystem: '<S16>/Get Parameter1'
20254
20255 // Start for MATLABSystem: '<S16>/Get Parameter2'
20256 cartesian_waypoints_planner_DW.obj_f.matlabCodegenIsDeleted = false;
20257 cartesian_waypoints_planner_DW.obj_f.isInitialized = 1;
20258 for (cartesian_waypoints_planner_B.i_k = 0;
20259 cartesian_waypoints_planner_B.i_k < 5;
20260 cartesian_waypoints_planner_B.i_k++) {
20261 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20262 tmp_b[cartesian_waypoints_planner_B.i_k];
20263 }
20264
20265 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20266 ParamGet_cartesian_waypoints_planner_302.initialize
20267 (cartesian_waypoints_planner_B.cv6);
20268 ParamGet_cartesian_waypoints_planner_302.initialize_error_codes(0, 1, 2, 3);
20269 ParamGet_cartesian_waypoints_planner_302.set_initial_value(0.3);
20270 cartesian_waypoints_planner_DW.obj_f.isSetupComplete = true;
20271
20272 // End of Start for MATLABSystem: '<S16>/Get Parameter2'
20273
20274 // Start for MATLABSystem: '<S16>/Get Parameter3'
20275 cartesian_waypoints_planner_DW.obj_e0.matlabCodegenIsDeleted = false;
20276 cartesian_waypoints_planner_DW.obj_e0.isInitialized = 1;
20277 for (cartesian_waypoints_planner_B.i_k = 0;
20278 cartesian_waypoints_planner_B.i_k < 5;
20279 cartesian_waypoints_planner_B.i_k++) {
20280 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20281 tmp_c[cartesian_waypoints_planner_B.i_k];
20282 }
20283
20284 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20285 ParamGet_cartesian_waypoints_planner_303.initialize
20286 (cartesian_waypoints_planner_B.cv6);
20287 ParamGet_cartesian_waypoints_planner_303.initialize_error_codes(0, 1, 2, 3);
20288 ParamGet_cartesian_waypoints_planner_303.set_initial_value(0.39);
20289 cartesian_waypoints_planner_DW.obj_e0.isSetupComplete = true;
20290
20291 // End of Start for MATLABSystem: '<S16>/Get Parameter3'
20292
20293 // Start for MATLABSystem: '<S16>/Get Parameter4'
20294 cartesian_waypoints_planner_DW.obj_a.matlabCodegenIsDeleted = false;
20295 cartesian_waypoints_planner_DW.obj_a.isInitialized = 1;
20296 for (cartesian_waypoints_planner_B.i_k = 0;
20297 cartesian_waypoints_planner_B.i_k < 5;
20298 cartesian_waypoints_planner_B.i_k++) {
20299 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20300 tmp_d[cartesian_waypoints_planner_B.i_k];
20301 }
20302
20303 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20304 ParamGet_cartesian_waypoints_planner_304.initialize
20305 (cartesian_waypoints_planner_B.cv6);
20306 ParamGet_cartesian_waypoints_planner_304.initialize_error_codes(0, 1, 2, 3);
20307 ParamGet_cartesian_waypoints_planner_304.set_initial_value(0.89);
20308 cartesian_waypoints_planner_DW.obj_a.isSetupComplete = true;
20309
20310 // End of Start for MATLABSystem: '<S16>/Get Parameter4'
20311
20312 // Start for MATLABSystem: '<S16>/Get Parameter5'
20313 cartesian_waypoints_planner_DW.obj_p.matlabCodegenIsDeleted = false;
20314 cartesian_waypoints_planner_DW.obj_p.isInitialized = 1;
20315 for (cartesian_waypoints_planner_B.i_k = 0;
20316 cartesian_waypoints_planner_B.i_k < 5;
20317 cartesian_waypoints_planner_B.i_k++) {
20318 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20319 tmp_e[cartesian_waypoints_planner_B.i_k];
20320 }
20321
20322 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20323 ParamGet_cartesian_waypoints_planner_305.initialize
20324 (cartesian_waypoints_planner_B.cv6);
20325 ParamGet_cartesian_waypoints_planner_305.initialize_error_codes(0, 1, 2, 3);
20326 ParamGet_cartesian_waypoints_planner_305.set_initial_value(0.2);
20327 cartesian_waypoints_planner_DW.obj_p.isSetupComplete = true;
20328
20329 // End of Start for MATLABSystem: '<S16>/Get Parameter5'
20330
20331 // Start for MATLABSystem: '<S16>/Get Parameter6'
20332 cartesian_waypoints_planner_DW.obj_l.matlabCodegenIsDeleted = false;
20333 cartesian_waypoints_planner_DW.obj_l.isInitialized = 1;
20334 for (cartesian_waypoints_planner_B.i_k = 0;
20335 cartesian_waypoints_planner_B.i_k < 5;
20336 cartesian_waypoints_planner_B.i_k++) {
20337 cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_k] =
20338 tmp_f[cartesian_waypoints_planner_B.i_k];
20339 }
20340
20341 cartesian_waypoints_planner_B.cv6[5] = '\x00';
20342 ParamGet_cartesian_waypoints_planner_306.initialize
20343 (cartesian_waypoints_planner_B.cv6);
20344 ParamGet_cartesian_waypoints_planner_306.initialize_error_codes(0, 1, 2, 3);
20345 ParamGet_cartesian_waypoints_planner_306.set_initial_value(-0.12);
20346 cartesian_waypoints_planner_DW.obj_l.isSetupComplete = true;
20347
20348 // End of Start for MATLABSystem: '<S16>/Get Parameter6'
20349
20350 // Start for MATLABSystem: '<S12>/Get Parameter'
20351 cartesian_waypoints_planner_DW.obj_es.matlabCodegenIsDeleted = false;
20352 cartesian_waypoints_planner_DW.obj_es.isInitialized = 1;
20353 for (cartesian_waypoints_planner_B.i_k = 0;
20354 cartesian_waypoints_planner_B.i_k < 10;
20355 cartesian_waypoints_planner_B.i_k++) {
20356 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_k] =
20357 tmp_g[cartesian_waypoints_planner_B.i_k];
20358 }
20359
20360 cartesian_waypoints_planner_B.cv3[10] = '\x00';
20361 ParamGet_cartesian_waypoints_planner_312.initialize
20362 (cartesian_waypoints_planner_B.cv3);
20363 ParamGet_cartesian_waypoints_planner_312.initialize_error_codes(0, 1, 2, 3);
20364 ParamGet_cartesian_waypoints_planner_312.set_initial_value(0.0);
20365 cartesian_waypoints_planner_DW.obj_es.isSetupComplete = true;
20366
20367 // End of Start for MATLABSystem: '<S12>/Get Parameter'
20368
20369 // Start for MATLABSystem: '<S12>/Get Parameter1'
20370 cartesian_waypoints_planner_DW.obj_d.matlabCodegenIsDeleted = false;
20371 cartesian_waypoints_planner_DW.obj_d.isInitialized = 1;
20372 for (cartesian_waypoints_planner_B.i_k = 0;
20373 cartesian_waypoints_planner_B.i_k < 10;
20374 cartesian_waypoints_planner_B.i_k++) {
20375 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_k] =
20376 tmp_h[cartesian_waypoints_planner_B.i_k];
20377 }
20378
20379 cartesian_waypoints_planner_B.cv3[10] = '\x00';
20380 ParamGet_cartesian_waypoints_planner_313.initialize
20381 (cartesian_waypoints_planner_B.cv3);
20382 ParamGet_cartesian_waypoints_planner_313.initialize_error_codes(0, 1, 2, 3);
20383 ParamGet_cartesian_waypoints_planner_313.set_initial_value(0.0);
20384 cartesian_waypoints_planner_DW.obj_d.isSetupComplete = true;
20385
20386 // End of Start for MATLABSystem: '<S12>/Get Parameter1'
20387
20388 // Start for MATLABSystem: '<S12>/Get Parameter2'
20389 cartesian_waypoints_planner_DW.obj_m.matlabCodegenIsDeleted = false;
20390 cartesian_waypoints_planner_DW.obj_m.isInitialized = 1;
20391 for (cartesian_waypoints_planner_B.i_k = 0;
20392 cartesian_waypoints_planner_B.i_k < 10;
20393 cartesian_waypoints_planner_B.i_k++) {
20394 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_k] =
20395 tmp_i[cartesian_waypoints_planner_B.i_k];
20396 }
20397
20398 cartesian_waypoints_planner_B.cv3[10] = '\x00';
20399 ParamGet_cartesian_waypoints_planner_314.initialize
20400 (cartesian_waypoints_planner_B.cv3);
20401 ParamGet_cartesian_waypoints_planner_314.initialize_error_codes(0, 1, 2, 3);
20402 ParamGet_cartesian_waypoints_planner_314.set_initial_value(0.0);
20403 cartesian_waypoints_planner_DW.obj_m.isSetupComplete = true;
20404
20405 // End of Start for MATLABSystem: '<S12>/Get Parameter2'
20406
20407 // Start for MATLABSystem: '<S12>/Get Parameter3'
20408 cartesian_waypoints_planner_DW.obj_h.matlabCodegenIsDeleted = false;
20409 cartesian_waypoints_planner_DW.obj_h.isInitialized = 1;
20410 for (cartesian_waypoints_planner_B.i_k = 0;
20411 cartesian_waypoints_planner_B.i_k < 10;
20412 cartesian_waypoints_planner_B.i_k++) {
20413 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_k] =
20414 tmp_j[cartesian_waypoints_planner_B.i_k];
20415 }
20416
20417 cartesian_waypoints_planner_B.cv3[10] = '\x00';
20418 ParamGet_cartesian_waypoints_planner_315.initialize
20419 (cartesian_waypoints_planner_B.cv3);
20420 ParamGet_cartesian_waypoints_planner_315.initialize_error_codes(0, 1, 2, 3);
20421 ParamGet_cartesian_waypoints_planner_315.set_initial_value(1.0);
20422 cartesian_waypoints_planner_DW.obj_h.isSetupComplete = true;
20423
20424 // End of Start for MATLABSystem: '<S12>/Get Parameter3'
20425
20426 // Start for MATLABSystem: '<S12>/Get Parameter4'
20427 cartesian_waypoints_planner_DW.obj_mf.matlabCodegenIsDeleted = false;
20428 cartesian_waypoints_planner_DW.obj_mf.isInitialized = 1;
20429 for (cartesian_waypoints_planner_B.i_k = 0;
20430 cartesian_waypoints_planner_B.i_k < 10;
20431 cartesian_waypoints_planner_B.i_k++) {
20432 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_k] =
20433 tmp_k[cartesian_waypoints_planner_B.i_k];
20434 }
20435
20436 cartesian_waypoints_planner_B.cv3[10] = '\x00';
20437 ParamGet_cartesian_waypoints_planner_316.initialize
20438 (cartesian_waypoints_planner_B.cv3);
20439 ParamGet_cartesian_waypoints_planner_316.initialize_error_codes(0, 1, 2, 3);
20440 ParamGet_cartesian_waypoints_planner_316.set_initial_value(1.0);
20441 cartesian_waypoints_planner_DW.obj_mf.isSetupComplete = true;
20442
20443 // End of Start for MATLABSystem: '<S12>/Get Parameter4'
20444
20445 // Start for MATLABSystem: '<S12>/Get Parameter5'
20446 cartesian_waypoints_planner_DW.obj_o.matlabCodegenIsDeleted = false;
20447 cartesian_waypoints_planner_DW.obj_o.isInitialized = 1;
20448 for (cartesian_waypoints_planner_B.i_k = 0;
20449 cartesian_waypoints_planner_B.i_k < 10;
20450 cartesian_waypoints_planner_B.i_k++) {
20451 cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_k] =
20452 tmp_l[cartesian_waypoints_planner_B.i_k];
20453 }
20454
20455 cartesian_waypoints_planner_B.cv3[10] = '\x00';
20456 ParamGet_cartesian_waypoints_planner_317.initialize
20457 (cartesian_waypoints_planner_B.cv3);
20458 ParamGet_cartesian_waypoints_planner_317.initialize_error_codes(0, 1, 2, 3);
20459 ParamGet_cartesian_waypoints_planner_317.set_initial_value(1.0);
20460 cartesian_waypoints_planner_DW.obj_o.isSetupComplete = true;
20461
20462 // End of Start for MATLABSystem: '<S12>/Get Parameter5'
20463 emxInitStruct_robotics_slmanip_(&cartesian_waypoints_planner_DW.obj);
20464 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_1);
20465 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_50);
20466 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_49);
20467 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_48);
20468 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_47);
20469 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_46);
20470 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_45);
20471 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_44);
20472 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_43);
20473 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_42);
20474 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_41);
20475 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_40);
20476 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_39);
20477 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_38);
20478 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_37);
20479 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_36);
20480 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_35);
20481 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_34);
20482 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_33);
20483 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_32);
20484 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_31);
20485 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_30);
20486 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_29);
20487 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_28);
20488 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_27);
20489 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_26);
20490 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_25);
20491 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_24);
20492 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_23);
20493 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_22);
20494 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_21);
20495 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_20);
20496 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_19);
20497 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_18);
20498 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_17);
20499 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_16);
20500 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_15);
20501 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_14);
20502 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_13);
20503 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_12);
20504 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_11);
20505 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_10);
20506 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_9);
20507 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_8);
20508 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_7);
20509 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_6);
20510 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_5);
20511 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_4);
20512 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_3);
20513 emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_2);
20514 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_51);
20515 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_82);
20516 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_81);
20517 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_80);
20518 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_79);
20519 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_78);
20520 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_77);
20521 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_76);
20522 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_75);
20523 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_74);
20524 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_73);
20525 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_72);
20526 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_71);
20527 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_70);
20528 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_69);
20529 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_68);
20530 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_67);
20531 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_66);
20532 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_65);
20533 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_64);
20534 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_63);
20535 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_62);
20536 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_61);
20537 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_60);
20538 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_59);
20539 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_58);
20540 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_57);
20541 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_56);
20542 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_55);
20543 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_54);
20544 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_53);
20545 emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_52);
20546 emxInitStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_83);
20547 emxInitStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_84);
20548 emxInitStruct_f_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_85);
20549 emxInitStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_86);
20550 emxInitStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_87);
20551 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_88);
20552 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_103);
20553 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_102);
20554 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_101);
20555 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_100);
20556 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_99);
20557 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_98);
20558 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_97);
20559 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_96);
20560 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_95);
20561 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_94);
20562 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_93);
20563 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_92);
20564 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_91);
20565 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_90);
20566 emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_89);
20567
20568 // Start for MATLABSystem: '<S3>/MATLAB System'
20569 cartesian_waypoints_planner_DW.obj.IKInternal.matlabCodegenIsDeleted = true;
20570 cartesian_waypoints_planner_DW.obj.matlabCodegenIsDeleted = true;
20571 cartesian_wa_eml_rand_mt19937ar(cartesian_waypoints_planner_DW.state_m);
20572 cartesian_waypoints_planner_DW.obj.isInitialized = 0;
20573 cartesian_waypoints_planner_DW.obj.matlabCodegenIsDeleted = false;
20574 obj = &cartesian_waypoints_planner_DW.obj;
20575 cartesian_waypoints_planner_DW.obj.isInitialized = 1;
20576 c_RigidBodyTree_RigidBodyTree_e
20577 (&cartesian_waypoints_planner_DW.obj.TreeInternal,
20578 &cartesian_waypoints_planner_DW.gobj_90,
20579 &cartesian_waypoints_planner_DW.gobj_91,
20580 &cartesian_waypoints_planner_DW.gobj_92,
20581 &cartesian_waypoints_planner_DW.gobj_93,
20582 &cartesian_waypoints_planner_DW.gobj_94,
20583 &cartesian_waypoints_planner_DW.gobj_95,
20584 &cartesian_waypoints_planner_DW.gobj_96,
20585 &cartesian_waypoints_planner_DW.gobj_89);
20586 obj_0 = &cartesian_waypoints_planner_DW.obj.IKInternal;
20587 obj_0->isInitialized = 0;
20588 inverseKinematics_set_RigidBody(obj_0, &obj->TreeInternal,
20589 &cartesian_waypoints_planner_DW.gobj_69,
20590 &cartesian_waypoints_planner_DW.gobj_70,
20591 &cartesian_waypoints_planner_DW.gobj_71,
20592 &cartesian_waypoints_planner_DW.gobj_72,
20593 &cartesian_waypoints_planner_DW.gobj_73,
20594 &cartesian_waypoints_planner_DW.gobj_74,
20595 &cartesian_waypoints_planner_DW.gobj_75,
20596 &cartesian_waypoints_planner_DW.gobj_76,
20597 &cartesian_waypoints_planner_DW.gobj_77,
20598 &cartesian_waypoints_planner_DW.gobj_78,
20599 &cartesian_waypoints_planner_DW.gobj_79,
20600 &cartesian_waypoints_planner_DW.gobj_80,
20601 &cartesian_waypoints_planner_DW.gobj_81,
20602 &cartesian_waypoints_planner_DW.gobj_82,
20603 &cartesian_waypoints_planner_DW.gobj_51,
20604 &cartesian_waypoints_planner_DW.gobj_28,
20605 &cartesian_waypoints_planner_DW.gobj_29,
20606 &cartesian_waypoints_planner_DW.gobj_30,
20607 &cartesian_waypoints_planner_DW.gobj_31,
20608 &cartesian_waypoints_planner_DW.gobj_32,
20609 &cartesian_waypoints_planner_DW.gobj_33,
20610 &cartesian_waypoints_planner_DW.gobj_34,
20611 &cartesian_waypoints_planner_DW.gobj_35,
20612 &cartesian_waypoints_planner_DW.gobj_36,
20613 &cartesian_waypoints_planner_DW.gobj_37,
20614 &cartesian_waypoints_planner_DW.gobj_38,
20615 &cartesian_waypoints_planner_DW.gobj_39,
20616 &cartesian_waypoints_planner_DW.gobj_40,
20617 &cartesian_waypoints_planner_DW.gobj_41,
20618 &cartesian_waypoints_planner_DW.gobj_42,
20619 &cartesian_waypoints_planner_DW.gobj_43,
20620 &cartesian_waypoints_planner_DW.gobj_44,
20621 &cartesian_waypoints_planner_DW.gobj_45,
20622 &cartesian_waypoints_planner_DW.gobj_46,
20623 &cartesian_waypoints_planner_DW.gobj_47,
20624 &cartesian_waypoints_planner_DW.gobj_48,
20625 &cartesian_waypoints_planner_DW.gobj_49,
20626 &cartesian_waypoints_planner_DW.gobj_50,
20627 &cartesian_waypoints_planner_DW.gobj_1,
20628 &cartesian_waypoints_planner_DW.gobj_27,
20629 &cartesian_waypoints_planner_DW.gobj_68,
20630 &cartesian_waypoints_planner_DW.gobj_83);
20631 obj_0->Solver = DampedBFGSwGradientProjection_D
20632 (&cartesian_waypoints_planner_DW.gobj_87);
20633 obj_1 = obj_0->Solver;
20634 obj_1->MaxNumIteration = 1500.0;
20635 obj_1->MaxTime = 10.0;
20636 obj_1->GradientTolerance = 1.0E-7;
20637 obj_1->SolutionTolerance = 1.0E-6;
20638 obj_1->ConstraintsOn = true;
20639 obj_1->RandomRestart = false;
20640 obj_1->StepTolerance = 1.0E-14;
20641 obj_0->matlabCodegenIsDeleted = false;
20642 emxInitStruct_robotics_slmani_e(&cartesian_waypoints_planner_DW.obj_e);
20643 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_1_b);
20644 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_16_b);
20645 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_15_o);
20646 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_14_a);
20647 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_13_o);
20648 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_12_h);
20649 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_11_f);
20650 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_10_n);
20651 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_9_d);
20652 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_8_g);
20653 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_7_a);
20654 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_6_d);
20655 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_5_d);
20656 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_4_p);
20657 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_3_d);
20658 emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_2_h);
20659
20660 // Start for MATLABSystem: '<S2>/MATLAB System'
20661 cartesian_waypoints_planner_DW.obj_e.isInitialized = 0;
20662 cartesian_waypoints_planner_DW.obj_e.isInitialized = 1;
20663 car_RigidBodyTree_RigidBodyTree
20664 (&cartesian_waypoints_planner_DW.obj_e.TreeInternal,
20665 &cartesian_waypoints_planner_DW.gobj_2_h,
20666 &cartesian_waypoints_planner_DW.gobj_4_p,
20667 &cartesian_waypoints_planner_DW.gobj_5_d,
20668 &cartesian_waypoints_planner_DW.gobj_6_d,
20669 &cartesian_waypoints_planner_DW.gobj_7_a,
20670 &cartesian_waypoints_planner_DW.gobj_8_g,
20671 &cartesian_waypoints_planner_DW.gobj_9_d,
20672 &cartesian_waypoints_planner_DW.gobj_3_d);
20673
20674 // Start for MATLABSystem: '<Root>/Get Parameter'
20675 cartesian_waypoints_planner_DW.obj_it.matlabCodegenIsDeleted = false;
20676 cartesian_waypoints_planner_DW.obj_it.isInitialized = 1;
20677 for (cartesian_waypoints_planner_B.i_k = 0;
20678 cartesian_waypoints_planner_B.i_k < 12;
20679 cartesian_waypoints_planner_B.i_k++) {
20680 cartesian_waypoints_planner_B.cv1[cartesian_waypoints_planner_B.i_k] =
20681 tmp_m[cartesian_waypoints_planner_B.i_k];
20682 }
20683
20684 cartesian_waypoints_planner_B.cv1[12] = '\x00';
20685 ParamGet_cartesian_waypoints_planner_378.initialize
20686 (cartesian_waypoints_planner_B.cv1);
20687 ParamGet_cartesian_waypoints_planner_378.initialize_error_codes(0, 1, 2, 3);
20688 ParamGet_cartesian_waypoints_planner_378.set_initial_value(10.0);
20689 cartesian_waypoints_planner_DW.obj_it.isSetupComplete = true;
20690
20691 // End of Start for MATLABSystem: '<Root>/Get Parameter'
20692
20693 // Start for MATLABSystem: '<Root>/Get Parameter1'
20694 cartesian_waypoints_planner_DW.obj_k.matlabCodegenIsDeleted = false;
20695 cartesian_waypoints_planner_DW.obj_k.isInitialized = 1;
20696 for (cartesian_waypoints_planner_B.i_k = 0;
20697 cartesian_waypoints_planner_B.i_k < 12;
20698 cartesian_waypoints_planner_B.i_k++) {
20699 cartesian_waypoints_planner_B.cv1[cartesian_waypoints_planner_B.i_k] =
20700 tmp_n[cartesian_waypoints_planner_B.i_k];
20701 }
20702
20703 cartesian_waypoints_planner_B.cv1[12] = '\x00';
20704 ParamGet_cartesian_waypoints_planner_379.initialize
20705 (cartesian_waypoints_planner_B.cv1);
20706 ParamGet_cartesian_waypoints_planner_379.initialize_error_codes(0, 1, 2, 3);
20707 ParamGet_cartesian_waypoints_planner_379.set_initial_value(1.0);
20708 cartesian_waypoints_planner_DW.obj_k.isSetupComplete = true;
20709
20710 // End of Start for MATLABSystem: '<Root>/Get Parameter1'
20711
20712 // Start for MATLABSystem: '<Root>/Get Parameter2'
20713 cartesian_waypoints_planner_DW.obj_fn.matlabCodegenIsDeleted = false;
20714 cartesian_waypoints_planner_DW.obj_fn.isInitialized = 1;
20715 for (cartesian_waypoints_planner_B.i_k = 0;
20716 cartesian_waypoints_planner_B.i_k < 12;
20717 cartesian_waypoints_planner_B.i_k++) {
20718 cartesian_waypoints_planner_B.cv1[cartesian_waypoints_planner_B.i_k] =
20719 tmp_o[cartesian_waypoints_planner_B.i_k];
20720 }
20721
20722 cartesian_waypoints_planner_B.cv1[12] = '\x00';
20723 ParamGet_cartesian_waypoints_planner_380.initialize
20724 (cartesian_waypoints_planner_B.cv1);
20725 ParamGet_cartesian_waypoints_planner_380.initialize_error_codes(0, 1, 2, 3);
20726 ParamGet_cartesian_waypoints_planner_380.set_initial_value(0.5);
20727 cartesian_waypoints_planner_DW.obj_fn.isSetupComplete = true;
20728
20729 // End of Start for MATLABSystem: '<Root>/Get Parameter2'
20730
20731 // Start for MATLABSystem: '<S13>/Set Parameter'
20732 cartesian_waypoints_planner_DW.obj_fb.matlabCodegenIsDeleted = false;
20733 cartesian_waypoints_planner_DW.obj_fb.isInitialized = 1;
20734 for (cartesian_waypoints_planner_B.i_k = 0;
20735 cartesian_waypoints_planner_B.i_k < 11;
20736 cartesian_waypoints_planner_B.i_k++) {
20737 cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_k] =
20738 tmp_p[cartesian_waypoints_planner_B.i_k];
20739 }
20740
20741 cartesian_waypoints_planner_B.cv2[11] = '\x00';
20742 ParamSet_cartesian_waypoints_planner_364.initialize
20743 (cartesian_waypoints_planner_B.cv2);
20744 cartesian_waypoints_planner_DW.obj_fb.isSetupComplete = true;
20745
20746 // End of Start for MATLABSystem: '<S13>/Set Parameter'
20747
20748 // Start for MATLABSystem: '<S13>/Set Parameter1'
20749 cartesian_waypoints_planner_DW.obj_dr.matlabCodegenIsDeleted = false;
20750 cartesian_waypoints_planner_DW.obj_dr.isInitialized = 1;
20751 for (cartesian_waypoints_planner_B.i_k = 0;
20752 cartesian_waypoints_planner_B.i_k < 11;
20753 cartesian_waypoints_planner_B.i_k++) {
20754 cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_k] =
20755 tmp_q[cartesian_waypoints_planner_B.i_k];
20756 }
20757
20758 cartesian_waypoints_planner_B.cv2[11] = '\x00';
20759 ParamSet_cartesian_waypoints_planner_365.initialize
20760 (cartesian_waypoints_planner_B.cv2);
20761 cartesian_waypoints_planner_DW.obj_dr.isSetupComplete = true;
20762
20763 // End of Start for MATLABSystem: '<S13>/Set Parameter1'
20764
20765 // Start for MATLABSystem: '<S13>/Set Parameter2'
20766 cartesian_waypoints_planner_DW.obj_a5.matlabCodegenIsDeleted = false;
20767 cartesian_waypoints_planner_DW.obj_a5.isInitialized = 1;
20768 for (cartesian_waypoints_planner_B.i_k = 0;
20769 cartesian_waypoints_planner_B.i_k < 11;
20770 cartesian_waypoints_planner_B.i_k++) {
20771 cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_k] =
20772 tmp_r[cartesian_waypoints_planner_B.i_k];
20773 }
20774
20775 cartesian_waypoints_planner_B.cv2[11] = '\x00';
20776 ParamSet_cartesian_waypoints_planner_366.initialize
20777 (cartesian_waypoints_planner_B.cv2);
20778 cartesian_waypoints_planner_DW.obj_a5.isSetupComplete = true;
20779
20780 // End of Start for MATLABSystem: '<S13>/Set Parameter2'
20781
20782 // Start for MATLABSystem: '<S13>/Set Parameter3'
20783 cartesian_waypoints_planner_DW.obj_hu.matlabCodegenIsDeleted = false;
20784 cartesian_waypoints_planner_DW.obj_hu.isInitialized = 1;
20785 for (cartesian_waypoints_planner_B.i_k = 0;
20786 cartesian_waypoints_planner_B.i_k < 11;
20787 cartesian_waypoints_planner_B.i_k++) {
20788 cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_k] =
20789 tmp_s[cartesian_waypoints_planner_B.i_k];
20790 }
20791
20792 cartesian_waypoints_planner_B.cv2[11] = '\x00';
20793 ParamSet_cartesian_waypoints_planner_371.initialize
20794 (cartesian_waypoints_planner_B.cv2);
20795 cartesian_waypoints_planner_DW.obj_hu.isSetupComplete = true;
20796
20797 // End of Start for MATLABSystem: '<S13>/Set Parameter3'
20798
20799 // Start for MATLABSystem: '<S13>/Set Parameter4'
20800 cartesian_waypoints_planner_DW.obj_o1.matlabCodegenIsDeleted = false;
20801 cartesian_waypoints_planner_DW.obj_o1.isInitialized = 1;
20802 for (cartesian_waypoints_planner_B.i_k = 0;
20803 cartesian_waypoints_planner_B.i_k < 11;
20804 cartesian_waypoints_planner_B.i_k++) {
20805 cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_k] =
20806 tmp_t[cartesian_waypoints_planner_B.i_k];
20807 }
20808
20809 cartesian_waypoints_planner_B.cv2[11] = '\x00';
20810 ParamSet_cartesian_waypoints_planner_372.initialize
20811 (cartesian_waypoints_planner_B.cv2);
20812 cartesian_waypoints_planner_DW.obj_o1.isSetupComplete = true;
20813
20814 // End of Start for MATLABSystem: '<S13>/Set Parameter4'
20815
20816 // Start for MATLABSystem: '<S13>/Set Parameter5'
20817 cartesian_waypoints_planner_DW.obj_ap.matlabCodegenIsDeleted = false;
20818 cartesian_waypoints_planner_DW.obj_ap.isInitialized = 1;
20819 for (cartesian_waypoints_planner_B.i_k = 0;
20820 cartesian_waypoints_planner_B.i_k < 11;
20821 cartesian_waypoints_planner_B.i_k++) {
20822 cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_k] =
20823 tmp_u[cartesian_waypoints_planner_B.i_k];
20824 }
20825
20826 cartesian_waypoints_planner_B.cv2[11] = '\x00';
20827 ParamSet_cartesian_waypoints_planner_373.initialize
20828 (cartesian_waypoints_planner_B.cv2);
20829 cartesian_waypoints_planner_DW.obj_ap.isSetupComplete = true;
20830
20831 // End of Start for MATLABSystem: '<S13>/Set Parameter5'
20832 }
20833}
20834
20835// Model terminate function
20836void cartesian_waypoints_planner_terminate(void)
20837{
20838 // Terminate for MATLABSystem: '<S11>/Get Parameter'
20839 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_eq);
20840
20841 // Terminate for MATLABSystem: '<S15>/Get Parameter'
20842 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_i);
20843
20844 // Terminate for MATLABSystem: '<S15>/Get Parameter1'
20845 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_db);
20846
20847 // Terminate for MATLABSystem: '<S15>/Get Parameter2'
20848 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_oq);
20849
20850 // Terminate for MATLABSystem: '<S15>/Get Parameter3'
20851 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_esa);
20852
20853 // Terminate for MATLABSystem: '<S15>/Get Parameter4'
20854 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_f4);
20855
20856 // Terminate for MATLABSystem: '<S15>/Get Parameter5'
20857 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_en);
20858
20859 // Terminate for MATLABSystem: '<S15>/Get Parameter6'
20860 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_l0);
20861
20862 // Terminate for MATLABSystem: '<S16>/Get Parameter'
20863 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_mb);
20864
20865 // Terminate for MATLABSystem: '<S16>/Get Parameter1'
20866 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_c);
20867
20868 // Terminate for MATLABSystem: '<S16>/Get Parameter2'
20869 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_f);
20870
20871 // Terminate for MATLABSystem: '<S16>/Get Parameter3'
20872 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_e0);
20873
20874 // Terminate for MATLABSystem: '<S16>/Get Parameter4'
20875 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_a);
20876
20877 // Terminate for MATLABSystem: '<S16>/Get Parameter5'
20878 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_p);
20879
20880 // Terminate for MATLABSystem: '<S16>/Get Parameter6'
20881 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_l);
20882
20883 // Terminate for MATLABSystem: '<S12>/Get Parameter'
20884 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_es);
20885
20886 // Terminate for MATLABSystem: '<S12>/Get Parameter1'
20887 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_d);
20888
20889 // Terminate for MATLABSystem: '<S12>/Get Parameter2'
20890 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_m);
20891
20892 // Terminate for MATLABSystem: '<S12>/Get Parameter3'
20893 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_h);
20894
20895 // Terminate for MATLABSystem: '<S12>/Get Parameter4'
20896 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_mf);
20897
20898 // Terminate for MATLABSystem: '<S12>/Get Parameter5'
20899 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_o);
20900
20901 // Terminate for MATLABSystem: '<S3>/MATLAB System'
20902 matlabCodegenHandle_matlab_evqu(&cartesian_waypoints_planner_DW.obj);
20903 matlabCodegenHandle_matlabC_evq(&cartesian_waypoints_planner_DW.obj.IKInternal);
20904 emxFreeStruct_robotics_slmanip_(&cartesian_waypoints_planner_DW.obj);
20905 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_1);
20906 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_50);
20907 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_49);
20908 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_48);
20909 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_47);
20910 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_46);
20911 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_45);
20912 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_44);
20913 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_43);
20914 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_42);
20915 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_41);
20916 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_40);
20917 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_39);
20918 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_38);
20919 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_37);
20920 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_36);
20921 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_35);
20922 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_34);
20923 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_33);
20924 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_32);
20925 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_31);
20926 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_30);
20927 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_29);
20928 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_28);
20929 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_27);
20930 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_26);
20931 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_25);
20932 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_24);
20933 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_23);
20934 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_22);
20935 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_21);
20936 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_20);
20937 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_19);
20938 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_18);
20939 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_17);
20940 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_16);
20941 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_15);
20942 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_14);
20943 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_13);
20944 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_12);
20945 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_11);
20946 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_10);
20947 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_9);
20948 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_8);
20949 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_7);
20950 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_6);
20951 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_5);
20952 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_4);
20953 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_3);
20954 emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_2);
20955 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_51);
20956 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_82);
20957 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_81);
20958 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_80);
20959 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_79);
20960 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_78);
20961 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_77);
20962 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_76);
20963 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_75);
20964 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_74);
20965 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_73);
20966 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_72);
20967 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_71);
20968 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_70);
20969 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_69);
20970 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_68);
20971 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_67);
20972 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_66);
20973 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_65);
20974 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_64);
20975 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_63);
20976 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_62);
20977 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_61);
20978 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_60);
20979 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_59);
20980 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_58);
20981 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_57);
20982 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_56);
20983 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_55);
20984 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_54);
20985 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_53);
20986 emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_52);
20987 emxFreeStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_83);
20988 emxFreeStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_84);
20989 emxFreeStruct_f_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_85);
20990 emxFreeStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_86);
20991 emxFreeStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_87);
20992 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_88);
20993 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_103);
20994 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_102);
20995 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_101);
20996 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_100);
20997 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_99);
20998 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_98);
20999 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_97);
21000 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_96);
21001 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_95);
21002 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_94);
21003 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_93);
21004 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_92);
21005 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_91);
21006 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_90);
21007 emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_89);
21008 emxFreeStruct_robotics_slmani_e(&cartesian_waypoints_planner_DW.obj_e);
21009 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_1_b);
21010 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_16_b);
21011 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_15_o);
21012 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_14_a);
21013 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_13_o);
21014 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_12_h);
21015 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_11_f);
21016 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_10_n);
21017 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_9_d);
21018 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_8_g);
21019 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_7_a);
21020 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_6_d);
21021 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_5_d);
21022 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_4_p);
21023 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_3_d);
21024 emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_2_h);
21025
21026 // Terminate for Atomic SubSystem: '<Root>/Subscribe1'
21027 // Terminate for MATLABSystem: '<S10>/SourceBlock'
21028 matlabCodegenHandle_matl_evqusn(&cartesian_waypoints_planner_DW.obj_pt);
21029
21030 // End of Terminate for SubSystem: '<Root>/Subscribe1'
21031
21032 // Terminate for MATLABSystem: '<Root>/Get Parameter'
21033 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_it);
21034
21035 // Terminate for MATLABSystem: '<Root>/Get Parameter1'
21036 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_k);
21037
21038 // Terminate for MATLABSystem: '<Root>/Get Parameter2'
21039 matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_fn);
21040
21041 // Terminate for MATLABSystem: '<S13>/Set Parameter'
21042 cartesian_way_matlabCodegenHa_j(&cartesian_waypoints_planner_DW.obj_fb);
21043
21044 // Terminate for MATLABSystem: '<S13>/Set Parameter1'
21045 cartesian_way_matlabCodegenHa_j(&cartesian_waypoints_planner_DW.obj_dr);
21046
21047 // Terminate for MATLABSystem: '<S13>/Set Parameter2'
21048 cartesian_way_matlabCodegenHa_j(&cartesian_waypoints_planner_DW.obj_a5);
21049
21050 // Terminate for MATLABSystem: '<S13>/Set Parameter3'
21051 cartesian_way_matlabCodegenHa_j(&cartesian_waypoints_planner_DW.obj_hu);
21052
21053 // Terminate for MATLABSystem: '<S13>/Set Parameter4'
21054 cartesian_way_matlabCodegenHa_j(&cartesian_waypoints_planner_DW.obj_o1);
21055
21056 // Terminate for MATLABSystem: '<S13>/Set Parameter5'
21057 cartesian_way_matlabCodegenHa_j(&cartesian_waypoints_planner_DW.obj_ap);
21058
21059 // Terminate for Atomic SubSystem: '<Root>/Publish'
21060 // Terminate for MATLABSystem: '<S9>/SinkBlock'
21061 matlabCodegenHandle_matla_evqus(&cartesian_waypoints_planner_DW.obj_b);
21062
21063 // End of Terminate for SubSystem: '<Root>/Publish'
21064}
21065
21066//
21067// File trailer for generated code.
21068//
21069// [EOF]
21070//
21071